Compact Sensor System for Target 
Localization 


Stian Tafjord Hovde 





Thesis submitted for the degree of 
Master in Cybernetics 
30 credits 
Faculty of mathematics and natural sciences 
UNIVERSITY OF OSLO 


Spring 2017 


Compact Sensor System for Target 
Localization 


Stian Tafjord Hovde 


© 2017 Stian Tafjord Hovde 
Compact Sensor System for Target Localization 
http://www.duo.uio.no/ 


Printed: Reprosentralen, University of Oslo 


Abstract 


The main topic of this thesis is to design a compact sensor system that provides target localization 
functionality. The system should be low cost, small and lightweight, so it can be mounted on a 
standard assault rifle. To localize a given target, the system has to both determine its own position, 
and the distance and direction to the target. The most challenging part of this is to determine the 
heading. 

To find this heading, our system uses a dual antenna Global Navigation Satellite System (GNSS) 
receiver. Because of the risk of GNSS signals being jammed, our proposed system also includes 
a stereo camera and an inertial measurement unit (IMU). Together these are able to maintain 
heading reference for a period of time after the loss of GNSS signals. The data from our sensors 
are fused in an error state Kalman filter implemented in Navigation Laboratory (NavLab). 

Test results show that with the short antenna baseline available in this system, the GNSS 
receiver still gives a good heading estimate, if we compensate for receiver bias. Results also show 
that addition of the stereo camera reduces the heading drift by a factor of at least 2. In the worst 
case scenario, an acceptable heading is maintained for up to 5 minutes without GNSS, with some 
assumptions. 

It is concluded that the designed sensor system is a viable device to extract target coordinates 
in the field. The results achieved in this thesis provide a good foundation for further work on this 
subject. 
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Chapter 1 


Introduction 


1.1 Motivation 


In the armed forces there is a need for soldiers in the field to be able to localize targets and determine 
target coordinates when requesting indirect fire support. Most of these soldiers would have target 
localization as a secondary role, as opposed to soldiers within artillery or mortar divisions. 

Today, these second line target localizers are not equipped with advanced fire control systems 
to determine target coordinates, but instead rely on maps to estimate the latitude, longitude and 
height of the target. It is therefore a need to provide these soldiers with equipment that can be 
used to determine target coordinates in a fast and precise way. On the other hand, giving the 
soldier more equipment to carry and control may be a disadvantage unless the equipment is well 
integrated. 

It is therefore of interest to investigate if the equipment the soldier is already carrying can be 
modified to give this functionality, with minor increase in weight. The cost of the system should 
also reflect that the target localization in this case is a secondary task. 

The suggestion proposed by the Norwegian Defence Research Establishment (FFT) was to equip 
a standard assault rifle with a dual antenna Global Navigation Satellite System (GNSS) receiver, 
placing one antenna in each end of the rifle, to provide an accurate position and heading estimate 
for the soldier. This system will be reliant on GNSS signals, and since jamming of these signals 
has been quite common in modern battlefields, a robust support system is required. 

The resulting idea for this thesis is then to create a proof-of-concept version of such a system. 
In addition to the dual antenna GNSS, an inertial measurement unit (IMU) and a camera are 
used to provide navigation when GNSS is unavailable. A requirement for the system is that the 
total target localization error should not exceed 30 m circular error probable (CEP), so it needs 
to be determined at what ranges and under which conditions we are able to achieve this. Target 
localization in the field is mostly done up to a range of about 2000 meters. Another requirement is 
for the total system to not add more weight than a standard under-barrel grenade launcher, about 
1.5 kg. 


1.2 Purpose and Goals 


The overall goal of this thesis is to create a proof-of-concept version of a compact sensor system 
that can localize a given target. It should be small and lightweight so as not to interfere with 
normal use when mounted on a standard assault rifle. 

When determining the location of a target, there are four main parameters one needs to de- 
termine: ones own position, and the heading, elevation and distance to the target. The position, 
elevation and distance can easily be determined using GNSS, accelerometers and laser rangefinders 
respectively. As will be discussed later, the heading is not that easy to determine. The heading 
error is also one of the largest contributors to the total target localization error. The primary focus 
of the thesis will therefore be to estimate the heading as accurately as possible. 

The individual goals can be roughly summed up in the following points: 
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1. Find a method of logging and synchronising the data from our different sensors. 


2. Build a testing platform where all our sensors can be mounted, to do realistic tests in the 
field. 


3. Fuse the data from our different sensors, to get an optimal heading estimate from our system. 


4. Test the dual antenna GNSS receiver and see how good a heading estimate we can get with 
different baseline lengths, and how it reacts to jamming. 


5. Test the complete system and analyse the data. 


1.3 Outline 


The thesis has the following structure: 
Chapter 2: Background: Presents the theoretical background used in this thesis. 


Chapter 3: Platform: Presents the testing platform constructed for this project, including 
hardware and software. 


Chapter 4: Sensor Models: Description of the mathematical sensor models implemented in 
NavLab. 


Chapter 5: Results: Description of the tests performed with our platform, and presentation 
of the results from these tests. 


Chapter 6: Discussion: Discussion of the testing results, and recommendations for further 
work. 


Chapter 7: Conclusion: Summary of goals achieved, and the final conclusion. 


Appendix A: Navigation Equations: Appendix with the navigation equations and their 
error propagation used in NavLab. 


Appendix B: Code Listings: Appendix with the Matlab and C++ code used in the thesis. 


Chapter 2 


Background 


This chapter presents some of the theoretical background used in this thesis. The material pre- 
sented here serves as an introduction on the topic, a deeper explanation can be found in the 


provided references. 


2.1 Notation 


The notation used in this thesis is based on [9]. The general notation is presented in Table 2.1. 





Symbol Description 








Scalar: lower case letter 





Coordinate frame: upper case letter 





Matrix: bold upper case letter 





a y »je 


Coordinate free/geometrical vector: bold lower case letter 





> 


bold lower 
superscript 


8 








Vector decomposed/represented in a specific coordinate frame: 


case letter with the coordinate frame as a right 





Table 2.1: General notation used in the thesis. 





In the physical world, we use vectors to describe quantities like position, angular velocity etc. 
These quantities relate one coordinate frame to another, and to make the quantity unique, the two 
frames in question are given as a right subscript, as seen in Table 2.2. 



































Name Symbol | Description 
Position vectör Pus A vector whose length and direction is such that it extends from 
the origin of frame A to the origin of frame B. 
'The velocity of the origin of coordinate frame B relative to coor- 
Velocity vector Van dinate frame A. Underline shows that both the orientation and 
position of A matters. 
The acceleration of the origin of coordinate frame B relative to 
Acceleration vector Gap coordinate frame A. Underline shows that both the orientation 
and position of A matters. 
] a A 3x3 direction cosine matrix describing the orientation of frame 
Rotation matrix Ras ; 
B relative to frame A. 
Angular velocity Wan The angular velocity of frame B relative to frame A. 
A vector describing the difference between R,, and Raz, inter- 
Error vector Cap peted as the error in Rap. 





Table 2.2: Symbols used to describe basic relations between two coordinate frames [9]. 


Note that this notation follows the “rule of closest frames”. This means that when transforming 
quantities between different coordinate frames, the “closest” frames in the equation must be the 
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same one (e.g. to get the rotation matrix R,., you multiply R,, with R^. Here we see that the 
B frames are “closest” ). 

The vectors in Table 2.2 are written in a coordinate free form. Before implementation on a 
computer, they must be decomposed in a selected coordinate frame (e.g. the position vector Das 
decomposed in frame C is p§,). 

Until now, three arbitrary frames A, B and C has been used as an example. In the main part 
of this thesis the used coordinate frames will have a meaning given by Table 2.3 





Coordinate frame | Description 








I Inertial space 

Earth-fixed coordinate frame, moves and rotates with the Earth. 
The x-axis is along the Earth’s rotation axis, pointing north (the 
yz-plane coincides with the equatorial plane), the y-axis points 
towards longitude +90° (east). 

Body-fixed coordinate frame, attached to the navigating vehicle. 
B The x-axis points forward, the y-axis to the right (starboard) and 
the z-axis in the vehicle’s down direction. 

Local coordinate frame, with the origin directly above or below 
B, at Earth’s surface. The z-axis is pointing down. Initially, the 
x-axis points towards north, and the y-axis points towards east, 
but as the vehicle moves they are not rotating about the z-axis 
(their angular velocity relative to the Earth has zero component 
along the z-axis). 





E 




















Table 2.3: Coordinate frames in use [9]. 


2.2 Northfinding 


As discussed in the introduction, finding an accurate heading estimate is one of the most important 
goals for our system. The reason for this can be explained by looking at Figure 2.1. 


Position error 
—> 
System 
position P1 P2 P3 PA 






Heading error 


System 
position P1 P2 P3 P4 


Figure 2.1: Investigating the position error caused by heading error. 


Assume our system is standing at the “system position” mark. We can see that any position 
error we have for our starting position will lead to a constant error in our estimation of the targets 
position, no matter the distance to our target. On the other hand, we can see that a heading error 
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will cause target position error to increase based on the distance. For example, a 1 degree heading 
error will at 1 km cause a target position error of 17.5 m, while at 2 km this increases to 35 m. 

It is therefore obvious that an accurate estimate of our systems heading is vital for us to get 
an accurate estimate of the position of the target we are trying to localize. 


There are seven main different ways to determine the heading of an object [10]. For convenience 
these are listed in Table 2.4. 









































Method Vector in use | Notes 
i: Magnetic commpacs T Increasing latitude 
decreases accuracy 
Ea 5 Increasing latitude 
ni d nd decreases accuracy 
3. Observing multiple external objects Do,o; 
4. Measure bearing to object with known position Pao GNSS required 
5. Multi-antenna GNSS Pp, Bo GNSS required 
; . GNSS required 
6. Vehicle velocity VER Vehicle motion required 
: ; GNSS required 
7. Vehicle acceleration Anz Vehicle iiotiomfeguied 








Table 2.4: Summary of the methods to find heading [10]. 


We can now walk through the list of methods and see which ones are suitable to find the heading 
for our system: 


e Method 1: No; a magnetic compass would be sensitive to magnetic materials in the ground, 
on our rifle, and attached to the soldier, and would therefore be too inaccurate for our 
purpose. 


e Method 2: No; a gyrocompass that gives us an accurate enough estimate of the heading 
would be too heavy and too expensive. 


e Method 3: Sometimes; can be used when multiple recognisable objects can be seen (e.g. 
celestial objects during clear weather). 


e Method 4: Sometimes; can be used when a recognisable object can be seen. 


e Method 5: Maybe; it is uncertain whether the baseline length between antennas achievable 
on a soldiers rifle is long enough to give a good heading estimate. 


e Method 6: Maybe; it is uncertain if the velocity exerted by a soldier is high enough to give 
a good heading estimate. 








e Method 7: Maybe; it is uncertain if the acceleration exerted by a soldier is high enough to 
give a good heading estimate. 


As we can see, there is not a simple way to give an accurate heading estimate with our system 
in all situations. As method 3 and 4 are only available when recognisable objects are spotted, we 
will not be using these in our thesis. Also, since we are focused on getting an accurate heading 
measurement, our system will not be in translational motion during the testing. We will therefore 
not be able to measure any velocity or acceleration, and will thus not investigate method 6 or 7. 
This leaves us with method 5, a multi-antenna GNSS system, which we will be looking at more 
thoroughly in this thesis. 

The drawback with the multi-antenna GNSS method, is that heading accuracy depends on the 
length of the baseline between the antennas. The maximum baseline we can have on a standard 
assault rifle is around 0.5 meters. It needs to be investigated if this baseline is sufficient, or if the 
baseline available to us on the rifle has to be extended by some means. 
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2.3 Rotation Matrix 


The material in this section is based on [12] and [6]. A rotation matrix is a 3x3 direction cosine 
matrix that describes the orientation of one frame in relation to another. One way to formulate 
a general rotation matrix R4 is the following: The coordinate frame A is first rotated around 
its own x-axis with an angle of 0,, then an angle 0, around its new y-axis, and finally an angle 0, 
around its new z-axis. The coordinate frame we end up with after all these rotations is B. The 
rotation matrix can therefore be expressed as: 


1 0 0 c(0,. 0 5s(0,)| [c(0,) —s(0,) 0 
Ras = |0 c(0,) —s(0,) 0 1 0 s(0,  c(0, 0 
0 s(0,)  c(0) —s(0,) 0 c(0,) 0 0 1 
(2.1) 
c(0,)c(0,) —c(04)s(0,) s(0.) 
= | s(01)s(0:)c(0.) + c(0,)s(03)  —s(0,)s(02)s(03) + c(0,)c(0,) —s(0,)c(9) 
-c(0,)5(0.)c(0,) + s(0,)s(0,)  c(0,)s(02)s(0,) + s(0,)c(0s) c(0,)c(0.) 


Here s(-) is the sine function and c(-) is the cosine function. The three parameters 0,, 0, and 
0, give a unique representation of R45, and we can therefore organise these in a vector: 


0,5 = 10, (2.2) 


Some key characteristics of the rotation matrix follows: 
To transform a vector represented in frame B to frame A we have that: 


Pap = Raspis (2.3) 


To create a new rotation matrix Ryo: 


Rac = Ras Rsc (2.4) 


Since we assume all our coordinate frames have orthonormal basis vectors, the rotation matrix 
will be an orthogonal matrix: 


Rap(Rap)” =I (2.5) 


If we look at the change in the rotation matrix over time, we can find the time derivative of 
equation 2.5: 


: . d 
Ras(Ras)” a Rais(Ras)” = al =0 (2.6) 
Equation 2.6 can be written: 
Rui Rs)" stt (este =0 (2.7) 
If we now define 
S = Ras(Ras)” (2.8) 
we have that: 
S+S7=0 (2.9) 


This means that S is a skew symmetric matrix, and there exists a vector wi, = [w,,w,, w;]* 


that fulfils: 


0 =w; We 
S(wi,) = | ws 0 =w, (2.10) 
-wW Ww: 0 


We interpret this vector as the angular velocity of the A system as seen from the B system. It 
can also be shown that the cross product operator can be written on the following form [12]: 
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wie x Cas = O je s (2.11) 


By multiplying R5 from the right with equation 2.6 we end up with: 


Ras + Ras (Rew) Ras =0 (2.12) 
using 2.8 we get: 
Ras => Ras(Ras)” Ras — —S* Rap =0 (2.13) 
Thus, we end up with: 
Ras = S(w4,) Rav = RasS(w*,) (2.14) 


It is also useful to know how a rotation matrix that is approximately equal to R,,;, denoted as 
Ras, can be expressed. From [6], we have that: 


Rap © (I + S(e4,)) Ran = Ran (1 + S(e5,)) (2.15) 


Where e, is the vector that describes the rotation needed to correct the approximate rotation 
matrix Ras to the true rotation matrix Rap. 

If we now assume that any rotation matrix we calculate can be written as the true rotation 
matrix plus some error: fu = Raz +0Raz, it is clear that: 


¿Rar = S(e^,)R,, = R4sS(e.,) (2.16) 


2.4 Stochastic Variables 


The material in this section is based on [4]. A stochastic, or random, variable X is in it simplest 
terms a variable which is assigned values at random. It can be described as a function of the 
outcomes of some random experiment. The probability for the stochastic variable to take a given 
value is defined by the probability distribution function F(x): 


F(x) = p(X € x) (2.17) 


or the probability density function f(x): 


f(x) = (2.18) 





The relation between these are: 


F(a) = f naidu (2.19) 


A stochastic variable has an expectation z and a variance o? defined as: 


oe ae T xf (x)de (2.20) 


o? =Var{X} = IN (x — ELX? f(z)dz = E{(X — 5)?) (2.21) 


The expectation, or mean value, is the average value a number of observations of X tend 
towards, as the number of observations increases. The variance of a random variable is the mean 
squared deviation of the random variable from its mean. The square root of the variance is known 
as the standard deviation. 

If a set of stochastic variables are independent, it holds that the variance of the sum of those 
stochastic variables is equal to the sum of the variances: 


c=) gr (2.22) 
i=l 
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Given two stochastic variables X and Y, the variance between these can be found. This is 
known as the covariance: 


Cov[X, Y) = E{(X — z)(Y - g)) (2.23) 


Throughout this thesis we will be using normal-distributed, or Gaussian, stochastic variables. 
If a variable X is Gaussian it is denoted as X ~ N(z,0?). The probability density function of a 
Gaussian variable is given as: 


f(z) =e 3 (2.24) 


2.5 1. Order Markov Process 


The material in this section is based on [4]. In Chapter 4 it is shown that our measurements is 
affected by coloured noise, also referred to as a bias, in addition to white noise. Modelling this 
bias as a 1. order Markov process has from experience at FFI shown to give a good model of the 
real noise. 

A continuous process x(t) is a 1. order Markov process if for every k, and for t, < tz < ... < ty, 
it is true that: 


Fle(ty)la(ty_1),---»e(ts)] = Flet elt) (2.25) 


That is, the probability distribution function for x(t,) is only dependent on the value one time 
step in the past. If the probability distribution function is dependent on values n time steps in the 
past, it is called a n-th order Markov process. 

If the continious process x(t) is a 1. order Markov process, it can be formulated as the differential 
equation: 


dr 1 
Rearranging Equation 2.26 yields: 
i l + (2.27) 
GSS 2 . 
T Y 


Where T' is the time constant, and y is the white noise which drives the bias. If the probability 
density function of y is Gaussian, the process x(t) is known as a 1. order Gauss-Markov process. 


2.6 Error State Kalman Filter 


One of our main goals is to find the best possible estimate of our platforms heading by fusing our 
sensor measurements in an optimal way. This is done using the error state Kalman filter (ESKF), 
also known as the indirect Kalman filter, in NavLab [8] (see Section 3.2.4). 

We first look into how the standard Kalman filter works. The Kalman filter is an optimal 
estimator for linear stochastic systems with additive zero mean Gaussian noise, derived in for 
example [2]. The information in these systems is first specified as a continuous-discrete state-space 
model: 


a(t) = F(t)x(t) + L(t)u(t) + G(t)v(t) (2.28) 


Equation 2.28 is called the process model, which models the transformation of the process state 
x(t) at time t. Equation 2.29 is called the measurement model, which describes the relationship 
between the process state 1, and the measurements y, at time step k. F is the system matrix, L 
is the control matrix, G is the process noise matrix and H is the measurement matrix. 

v(t) and £, is the process and measurement noise. These are white and uncorrelated, with 
spectral density matrix Q and covariance matrix W, respectively. v(t), €, and x(t.) is uncorrelated. 
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The Kalman filter is an recursive algorithm that combines sensor mesurements and the system 
information in an optimal way. Optimal here means that the algorithm finds a minimum variance 
estimate. Recursive means that the filter only need the estimated state and its covariance from 
the previous time step, and the current measurement, to get the estimate for the current state. 
The continuous-discrete Kalman filter algorithm is given in [2]: 


Zo = E(x) 
P, = Ello — £x, — Zo) | (2.30) 
z(t) = F(t)z(t) + L(t)u(t) T" 


P(t) = F(t)P(t) + POF) + G()0G" (t) 


K, = P.H? (H,P,H? + W.) 
Ey = Tr + Kalyn — Hi) (2.32) 
Ê, = (1- K,H,)P, 

Before implementation into a computer, the process model needs to be discretized. As NavLab 


handles this internally, this procedure will not be listed here, but is well explained in [6]. We then 
end up with the discrete process model: 


Cpa = Oa, + Apu, + OV, (2.33) 


where ®, A and Q are the discretized versions of matrices F, L and G respectively. We now 
end up with the discrete Kalman filter [2]: 


c -— (2.34) 
o = E|(zo — zo)(zo — zo): ] 
Tri = D,T, + A,u, (2.35) 


Pe = ©, P. d Q,Q,Q7 


K, = P,HT(H,P,HT --W,)-! 
&, = E, + Ki (y, — Hii) (2.36) 
Ê, = (I - K,H,)P, 


Here Equations 2.34 is the initialization step, where the a priori information of the state To 
and its covariance P, are input. Equations 2.35 is the prediction step, which uses the estimated 
state from the previous time step to calculate an estimated state at the current time step. This 
is also known as the a priori state estimate. Finally, Equations 2.36 is the update step, where the 
predicted state estimate is refined based on measurements. This is known as the a posteriori state 
estimate. 

Typically, the prediction and update steps alternate, with the prediction step bringing the 
state estimate to the next time step, and the update step refining it with information from the 
measurements. However, if the measurements are missing in some time steps, the update step is 
skipped, and several prediction steps can be performed instead. 

Note that during calculations in a computer, numerical errors might arise that makes the 
covariance matrix of the state vector not symmetrical and positive semidefinite. In these cases, the 
Kalman filter will not be optimal. Therefore, the covariance update equation in NavLab is written 
on what is called the Joseph form [13]: 


Ê, = (I — K,H,)P.( — K,H,)' + K,R,KT (2.37) 
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Since we have measurements of the states we are interested in, it is sufficient to estimate the 
error in these. The states in the Kalman filter then become error states, and we end up with the 
error state Kalman filter (ESKF). These error states are calculated using the difference between 
the inertial navigation system (IMU measurements and navigation equations) and external sources 
of data (e.g. GNSS, camera). 

Some advantages using the ESKF over a standard Kalman filter in conjunction with inertial 
navigation systems (INS) are [22]: 


e The standard Kalman filter using the total state representation has to incorporate a dynamic 
model of the vehicle, as well as attempt to suppress noisy and erroneous data at a relatively 
high frequency. Using the ESKF the INS is able to follow the high frequency motions very 
accurately, so there is no need to model these dynamics explicitly in the filter. 


e The dynamics involved in the total state description include a high frequency component 
and is only well described by a non-linear model. The dynamics upon which the ESKF is 
based are a set of inertial system error propagation equations which are low frequency and 
very adequately represented as linear. 


e If the standard Kalman filter fails, the entire navigation algorithm fails. The ESKF can in 
case of a failure continue to provide estimates by acting as an integrator on the INS data. 


e Because the ESKF is outside the INS loop and based on low frequency dynamics, its sampling 
rate can be much lower than a standard Kalman filter. 


For these reasons, the ESKF is almost always used in terrestrial aided inertial navigation 
systems. 


2.7 Camera Calibration 


Camera calibration is needed to find the intrinsic parameters of the specific camera that is being 
used. The calibration process also outputs the lens distortion parameters that we have to correct 
for, before running the visual navigation algorithms. 

The common way to accomplish this is to take multiple pictures with our cameras of a chess 
board with known size. This is known as Zhang's method [26]. The pictures should be taken 
from several different angles, and with varying size and rotation. Feeding these images into the 
calibration algorithm in for example OpenCV (see Section 3.2.2) or the camera_calibration 
package in ROS (see Section 3.2.1), will ouput the intrinsic matrix K and the distortion coefficients: 


Ju Eo 
K-—]|0 f, c,|, distortionCoefficients = [kı kə pi Po ka] (2.38) 
0 0 1 


In the intrinsic matrix, f, and f, are the camera focal lengths in pixels, c, and c, is the camera’s 
principal point (or optical centre) in pixels, and s is the skew coefficient, which is normally set to 
0 since the image axes are usually perpendicular. 

The distortion coefficients k,, k, and k, describe the radial distortion in the camera that 
manifests in form of the “barrel” or “fish-eye” effect. p, and p, describe the tangential distortion, 
which occurs when the image taking lenses are not perfectly parallel to the imaging plane. 

In the case of stereo cameras, the calibration procedure will also output the rotation and 
translation between the two cameras. 

Now that we have the intrinsic matrix and the distortion coefficients, we are able to rectify our 
images in for example OpenCV (see section 3.2.2), before sending them to the visual navigation 
algorithms. 
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Chapter 3 


Platform 


This chapter details the specific hardware and software used on our testing platform. It also 
shows the complete system, errors in mounting, and how the coordinate frames of the different 
components relate to each other. 


3.1 Hardware 
3.1.1 GNSS 


As explained in the introduction, we will be using a GNSS receiver with dual antenna input to 
measure the heading of our system accurately. The FlexPak6D from NovAtel has been used in this 
project. This is a light and compact receiver unit with several connection ports for easy access. 
The FlexPak6D tracks both GPS and GLONASS signals, as well as satellite-based augmentation 
system (SBAS) signals that may improve GNSS accuracy. 





Figure 3.1: FlexPak6D Dual Antenna GNSS Receiver [17]. 


Some key specifications of the FlexPak6D is listed in Table 3.1. These specifications are acquired 
from the FlexPak6D user manual [17]. The manual does not specify what kind of accuracy measure 
that are used for the different accuracy parameters. We will therefore assume they are given as 
standard deviations. 

As we can see from Table 3.1, the accuracy of the heading measurement is only listed with 
baselines of 4 and 2 meters. Since our baseline will be much shorter than 2 meters, we have to test 
how much this will affect heading accuracy. 

The FlexPak6d can output different data sets depending on user need. The full list of different 
log types can be found in the firmware reference manual [18]. We will use the bestpos log which 
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Name Value Unit 
Primary and secondary RF a /L2 [| 
Other signals SBAS [| 
Horizontal position accuracy L1/L2 1.2 [m] 
Heading accuracy 2m baseline 0.08 [deg] 
Heading accuracy 4m baseline 0.05 [deg] 
Size 147 x 113 x 45 [mm] 
Weight 315 [g] 

















Table 3.1: NovAtel FlexPak6D specifications [17]. 


outputs position (longditude, latitude and height), and the heading log which outputs platform 
heading relative to true north. 

Heading is determined by measuring signal phase difference between the two antennas from each 
satellite in view. The measurements are then used to calculate the vector between the reference 
antenna and the rover antenna. In this project the rear antenna is used as reference, so that the 
vector points forwards. To determine heading, the receiver uses the NovAtel ALIGN Heading and 
Relative Positioning GNSS Firmware [15]. 

To acquire the logs from the receiver, NovAtel has its own windows-based GUI called NovAtel 
Connect, that gives easy access the receiver via USB, to log data or change the configuration. We 
will also use a ROS software package to connect to the receiver and log data (see Section 3.2.1) 

Another point to note is that the rest of our system is using Unix time (or Unix epoch time), 
while our GNSS receiver outputs GPS time in GPS week and seconds into GPS week. We therefore 
have to do a quick conversion to Unix time. 

Unix time started at 1.1.1970 UTC, while GPS time started at 6.1.1980 UTC. This difference 
turns out to be 315964800 seconds. In addition, as of May 2017, 18 leap seconds has been added 
since the introduction of GPS time. There are also 604800 seconds in a week. Our formula to 
convert from GPS time to Unix time then becomes: 


unixTime = gpsW eek - 604800 + gpsSecond + 315964800 — 18 (3.1) 


In addition we also acquired two antennas of the type 42GOXX16A4-XT-1-1-Cert from NovAtel. 
These are compact antennas with dual frequency support, and combined GPS + GLONASS signal 
reception. Some key specifications of our antennas is listed in Table 3.2. These specifications are 
acquired from NovAtel's antennas brochure [16]. 


Figure 3.2: 42GOXX16A4-XT-1-1-Cert Antenna [16]. 
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Name Value Unit 


GPS: LI/L2 
BeiDou: B1/B2 








Signals received 








SBAS 
Size 119.38 x 76.20 x 22.80 | [mm] 
Weight 227 [el 




















Table 3.2: NovAtel 42GOXX16A4-XT-1-1-Cert specifications [16]. 


3.1.2 IMU 


An IMU is a device that uses gyroscopes and accelerometers to measure body angular rate and 
specific force. These measurements are integrated by the navigation equations listed in Appendix 
A, which outputs the position, orientation and velocity. 

The IMU we will be using is the MTi-100 from Xsens Technologies B.V. The MTi-100 is a 
micro electro mechanical system (MEMS) based IMU that contain a three-axis gyroscope and a 
three-axis accelerometer. It also contains a magnetometer and a barometer, although we at present 
do not plan to use measurements from these sensors. This IMU was chosen for its small size and 
low cost, and also based on recommendations from other projects at FFI. 





Figure 3.3: Xsens MTi-100 IMU [24]. 


Some key specifications of the MTi-100 is listed in Table 3.3. These specifications are acquired 
from the MTi-100 series user manual [24], and the calibration certificate received with the MTi-100. 
The manual and calibration certificate does not specify what kind of accuracy measure that are 
used for the different accuracy parameters. We will therefore assume they are given as standard 
deviations. 
































Name Value Unit 
Gyro full range 450 [deg/s] 
Gyro bias 0.5 [deg/s] 
Gyro measurement noise density 0.015 [deg/s/./Hz] 
Accelerometer full range 50 [m/s?] 
Accelerometer bias 0.03 [m/s?] 
Accelerometer measurement noise density 80 [ug/v Hz] 
Size 57 x 42 x 24 [mm] 
Weight 55 [el 

















Table 3.3: Xsens MTi-100 specifications [24]. 


Xsens also has its own logging program for its MTi platforms, called MT Manager. This runs on 
the Windows OS and allows both logging of data and configuration of the sensors. Unfortunately, 
this program only logs the start time to the nearest minute, which is too inaccurate for our purpose. 
We have therefore used ROS to log data from the IMU (see Section 3.2.1). 
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3.13 Camera 


Two different cameras have been used during this project. An inexpensive monocular webcam was 
used in the beginning to test out different functions and algorithms, while for the testing phase, a 
stereo camera was used. 

The switch from a mono to stereo camera was done because the different visual navigation 
algorithms cannot estimate motion or position on a metric scale with a monocular camera. The 
estimate becomes relative to some unknown scaling factor. This scale could be found using an IMU 
to estimate the motion carried out between each frame, but since we will be stationary during our 
tests, this will not work. With a stereo camera, we find the scale directly because we know the 
distance between the two camera centres, and can therefore triangulate points in the scene. 

The stereo camera rig consists of two cameras of the type Blackfly S Color 5.0 MP USB3 Vision 
(Sony IMX250), and two Xsens MTi-100 IMU’s, which are described in Section 3.1.2. A picture of 
the rig can be seen in Figure 3.4. This rig was previously built by FFI, and was made available in 
order to take synchronized image pairs during the testing of our system. The cameras are capable 
of capturing images at a rate up to 70 Hz. 





Figure 3.4: Stereo camera and IMU rig. 


This camera type uses a global shutter, as opposed to a rolling shutter, to capture images. This 
means that all photosites receive light at the same time, as opposed to row by row with a rolling 
shutter. The effects of a rolling shutter can best be seen e.g. when photographing the propeller of 
a plane. 

The cameras are fitted with fisheye lenses, providing a near 180° field of view (FOV). This 
makes them suited for visual navigation operations, since the image pairs from one time step to 
the next need to overlap in order for the algorithm to recognise feature points between them. At 
the same time, an increase in FOV lowers the pixel per degree resolution in the image, making the 
visual navigation algorithms more susceptible to noise. 

In order to capture the images, the rig is connected to its own computer running the Ubuntu 
Linux OS. The images are stored in .bin files, and the software to extract these was provided by 
FFI as Matlab code. After the extraction the images have been cropped to 1200 by 1200 pixels. 
We can now calculate the new FOV using the following equations [1]: 


h 1200 
=2 ae ee = Sail ee = OT? : 
fov, tan (=) 2tan (35) 98.27 (3.2) 
w 1200 
= 2t = — = 2 EU n = 24° $ 
fov, an (=) tan (s ; mi) 98 (3.3) 


Here h and w is the height and width of the image in pixels, f, and f, is the focal lengths 
found during the calibration procedure (see Section 2.7). 

To time stamp the image pairs, the cameras send a sync pulse to FFT's DSU logger, which pairs 
each pulse with time information from a GPS receiver. The time log is then stored, and can be put 
together with the image data at a later time. Because of the risk of GPS signals being jammed, 
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this method will not be acceptable for the final product. For further work, another way must be 
implemented to synchronise images to other data, for example by using ROS (see Section 3.2.1). 
The Matlab code to extract these time logs was also made available by FFI. 


3.2 Software 


3.2.1 ROS 


Robotic Operating System (ROS) is a collection of tools, libraries, and conventions that aim to 
simplify the task of creating complex and robust robot behavior across a wide variety of robotic 
platforms [21]. A ROS system consists of a number of independent packages, that communicate 
with each other using a publisher/subscriber system. For example, one of our sensors drivers can 
publish the sensor data on a topic, which can then be read by any number of subscribers. ROS is 
used in this project in order to easily connect to our different sensors, read the data they output 
and synchronise them. ROS is developed for the Ubuntu Linux OS. 

To connect to our IMU we use the xsens_driver package available in the ROS library. This 
package reads the data transmitted by the IMU through USB, and publishes it on a ROS message 
topic. The raw IMU data can now be sent to a filter, like the imu filter madgwick package, to 
calculate the orientation. As we are sending the raw IMU data directly into NavLab, this part is 
not implemented in the final result. 

For our GNSS receiver we had to write a new package, since this did not exist. NovAtel does 
provide a Linux USB driver though, so we are able to easily communicate with the receiver. The 
package written is shown in Listing B.3. This package opens up communications with the receiver, 
then transmits the commands for the receiver to start logging the data we need. It then receives 
this data and publishes it on a ROS message topic. 

In addition we have also run some tests of different ROS packages using our monocular web- 
cam. We start by reading images from the camera using the cv_camera package. We then run 
a calibration sequence as detailed in Section 2.7 with the camera_calibration package. To use 
the data from the calibration and rectify our images, we use the image_proc package. We tried to 
send the images to the viso2_ros package, which is the ROS wrapper for LIBVISO2 as explained 
in Section 3.2.3, to output odometry data. Due to the limitations of the monocular version of 
LIBVISO2, the resulting data was not used further. Since we used the stereo camera rig for our 
testing, this part is not implemented in the final result. 

An idea for future expansion into a real-time system is to send all the data we have now 
accumulated to the robot_localization package. This package accepts a multitude of sensor 
information, and combines them using an extended Kalman filter to give an estimate to the position 
of the object in question. As we are doing our estimation in NavLab, this has not been tested with 
our sensor data. 


Camera 


IMU 
Tested but not implemented 


, longitude, height & heading 
GNSS 





Figure 3.5: ROS implementation. 
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Figure 3.5 shows an overview of the packages we have used in ROS. It also shows the messages 
being published and subscribed by the different packages, and what parts that are implemented in 
the final result. 

Once all the data is being published on the different ROS topics, we can start logging them 
into a ROS bag file. The bag file format stores the ROS messages we want, and allows us to replay 
them whenever we need. This way we can quickly and easily store data out in the field, and then 
process it afterwards. 

For future work it might be useful to use the Robotic Toolbox available in Matlab. This toolbox 
has the functionality to do real time reading of ROS topics. It also has tools for working with ROS 
bag files. 


3.2.2 OpenCV 


OpenCV (Open Source Computer Vision Library) is an open source computer vision and machine 
learning software library [19]. The library has more than 2500 optimized algorithms, which includes 
a comprehensive set of both classic and state-of-the-art computer vision and machine learning 
algorithms. 

We will use a few of the OpenCV functions to work on our images before they are sent into the 
visual navigation algorithms. The OpenCV functions can be seen in action in Listing B.2, denoted 
with the cv namespace. As we are working on pictures from cameras with fisheye lenses, we must 
also use the fisheye namespace where applicable. 

The OpenCV library is also available in Matlab, using the mex functions in mexopencv [25]. 
An example of this in use can be seen in Listing B.1. As these mex functions do not contain the 
fisheye namespace, the rectification maps needed are directly imported from the C++ script. 

The following OpenCV functions have been used: 

stereoRectify: Inputs the intrinsic matrix, distortion coefficients and the rotation and trans- 
lation between the cameras found in the calibration procedure (see Section 2.7). Outputs the 
rectification transforms for each camera. These are two rotation matrices that that virtually make 
both camera image planes the same plane. It also outputs a projection matrix for each camera 
that transform points from the world frame into the new coordinates. Note that in OpenCV 3.2, 
fisheye::stereoRectify does not work properly. Therefore the results from this function is imported 
directly from OpenCV 3.1. 

init UndistortRectifyMaps: Inputs the intrinsic matrix, distortion coefficients and the rec- 
tification transform and projection matrix found in stereoRectify. Outputs the undistortion and 
rectification transformation map for each camera. These maps are used by the remap function to 
correct for the distortion, and transforms the images such that the epipolar lines on both images 
become horizontal and have the same y-axis coordinate. Horizontal epipolar lines helps the visual 
navigration algorithms to find feature points between the image pair. 

imread: Loads an image from the specified file and returns it. 

remap: Transforms the source image using the specified map. 

equalizeHist: Equalizes the histogram of a grayscale image. This normalizes the brightness 
and increases the contrast of the image, making it easier for the visual navigration algorithm to 
find feature points. 

medianBlur: Blurs the image using a median filter. Done to reduce the noise in the image. 


3.2.3 Visual Navigation Algorithms 


To get useful measurements from the camera, we need to run the pictures through an algorithm 
that is able to estimate and output the motion the camera experiences. Two different algorithms 
has been tested, and are detailed here. 


LIBVISO2 


LIBVISO2 (Library for Visual Odometry 2) is a very fast cross-platfrom C++ library with MAT- 
LAB wrappers for computing the 6 DOF motion of a moving mono/stereo camera [11]. A ROS 
wrapper for LIBVISO2 also exist, that reads directly from a ROS image topic and outputs odom- 
etry data. 

LIBVISO2 estimates the translation and rotation of either a monocular or stereo camera by 
extracting and matching corner-like features between two consecutive images. This means its 
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important for our images to overlap between time steps in order for the algorithm to recognise the 
same feature points. 

Additionally, LIBVISO2 uses an iterated sigma point Kalman filter to cope with non-linearities 
in the measurement equation. It also uses a random sample consensus outlier rejection scheme to 
detect and remove false matches, and features located on independently moving objects. 

To run LIBVISO2 with our monocular webcam, we use the ROS wrapper in the viso2_ros 
package. Due to the monocular version of LIBVISO2 assuming that the camera is moving at a 
known and fixed height over the ground, this run did not give any usable data. 

The Matlab script used to run LIBVISO2 with our stereo images is shown in Listing B.1. This 
is a modification of the stereo example script that is included with the Matlab wrappers. Before 
sending our images through the algorithm, they are rectified by the OpenCV functions. During 
testing, the algorithm estimated that our system had a constant velocity forwards, even though 
we were stationary. This error sounds like an error with the calibration procedure, although the 
same procedure has been used with ORB-SLAM2 and worked fine. As the source of this error was 
not identified, ORB-SLAM2 was instead used to extract data from our images. 


ORB-SLAM2 


ORB-SLAM2 is a real-time SLAM (Simultaneous Localization And Mapping) library for Monocu- 
lar, Stereo and RGB-D cameras that computes the camera trajectory and a sparse 3D reconstruc- 
tion [14]. In addition to solving the localization problem like in LIBVISO2, ORB-SLAM2 also 
solves the mapping problem: building a map of an unknown environment and localize the camera 
in it. ORB-SLAM2 is developed for the Ubuntu Linux OS. 

ORB-SLAM2 has three main threads that run in parallel: tracking, local mapping and loop 
closing. The tracking thread extracts and matches feature points in the scene to estimate the 
cameras position and orientation in the map. The local mapping thread manages the local map 
and optimizes it. Finally the loop closing thread detects if the camera has returned to a scene it 
has previously been at, and corrects the accumulated drift. 

The C++ script used to run ORB-SLAM? with stereo images is shown in Listing B.2. This is a 
modification of the stereo_kitty.cc example script that is included with the source files. Before 
sending our images through the algorithm, they are rectified by the OpenCV functions. Once the 
algorithm is finished, the data is stored in a text file that we can import into Matlab. 


3.2.4 NavLab 


NavLab (Navigation Laboratory) is a generic simulation and post-processing tool for navigation, 
developed by the navigation group at FFI [7], [8]. NavLab is built on object oriented Matlab, 
which allows us to create and add new classes to expand the functionality. In this thesis it is used 
for post-processing of data acquired from the different sensors in our system. 

As explained in section 2.6, NavLab uses an error state Kalman filter to estimate the errors in 
position and orientation of a system, using the difference between the IMU and external sources 
of data. In our case, the external data will be the position and yaw measurement from the GNSS 
receiver, and the delta angle measurement from the camera. 

The steps in NavLab can be broken down like this: measurements — preproc — estimator > 
export. 

Firstly we have to prepare our synchronised measurement data for input into NavLab. The 
preproc process accepts space-separated row vectors in the double format, gathered in a .bin file. 
The number of columns and what data they contain must be specified in the sensor_pre.ini file 
in NavLab. The “sensor” name is here replaced with the name of the sensor (e.g. “imu”). 

The preproc process is now able to read our sensor data. Preproc will split the data into .nlbin 
files that the estimator process reads, and also conduct any operations on the data that we might 
specify in the sensor_pre.ini file (e.g. rearrange the axes or compensate for lever arm). 

The estimator now reads our sensor data and use them, and our mathematical models of the 
sensor errors, to estimate the systems position, attitude and velocity. The estimator integrates 
the measurements from the IMU using the navigation equations listed in Appendix A. When a 
measurement from an additional sensor is available, the error state is calculated and sent as a 
measurement to the Kalman filter. 
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When the estimator is finished the data is ready to be exported, either through NavLab’s GUI, 
or directly in Matlab. It is also possible to run a smoothing process that uses the known future 
states to create a better estimate in the past. 

NavLab also has a variable quality option, meaning that with every single new measurement, 
the sensor parameters describing that particular measurement can be specified. In order to use 
this, a text file named sensor_est_quality.txt must be added to the measurement data folder. 

Note that since NavLab is copyrighted and sold commercially by Kongsberg Maritime, the code 
written for NavLab used in this thesis will not be listed here. 


3.3 Complete System 


Together with the prototype workshop at FFI, we have created a testing platform where we 
mounted the different sensors. This platform is a simple plank that is long enough to test several 
different baseline lengths between our antennas. It is also wide enough for our antennas to fit, and 
solid enough to withstand the weight and stress during testing. A picture of the platform with all 
sensors mounted can be seen in Figure 3.6. 





Figure 3.6: Complete test setup. 


Since the antennas have their connector on the bottom, four holes have been drilled in the 
platform to be able to mount them. The holes are drilled such that the baseline length of the 
antennas will be 0.5, 1 and 1.455 meters respectively. There are four screw holes in the corners of 
the antennas to fasten it to the platform. 

Our IMU is fastened in the centre of the platform on the top side. The IMU has three screw 
holes to secure it. 

The stereo camera and IMU rig is mounted upside-down on the bottom forward half of the 
platform, as this was the simplest way to install it. This means that the cameras are facing to the 
right. Two aluminium plates are used to fasten the rig to the platform. 

To be able to keep the platform upright and steady, threads have been put in the bottom centre 
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that fit the bolt of a tripod. This allows us to keep the platform fixed at a certain target, but also 
able to rotate it to change the heading. 

In addition to all this, we have also equipped the platform with makeshift iron sights to aid in 
aiming. On the back of the platform is a round metallic plate with a hole in it, and on the front 
is a nail sharpened to a point. To aim the platform, you line up the front nail with the back hole 
and the object you are aiming at. An example of this can be seen in Figure 5.2. 

Finally some handles have been mounted on the bottom to make the platform easier to carry 
when needed. 


3.3.1 Coordinate Frames 


Each of our sensors provide measurements represented in their own coordinate frames. We need to 
transform all of these measurements so they are represented in the body frame B. NavLab defines 
the body frame with the origin at the same position as the IMU’s origin, the x-axis pointing 
forward, the y-axis to the right and the z-axis pointing down. Here we have used IMU 1 from the 
stereo camera rig, since this was the one used during the testing. Figure 3.7 shows an overview of 
the different coordinate frames in our system. 


{B} Body Frame 






{IMU} IMU Frame 


{G} GNSS Frame 


{C} Camera Frame 


Figure 3.7: Coordinate frames in our system. 


If we observe the MTi-100 IMU with the connection port facing towards us, it uses the following 
coordinate system: the x-axis points forwards, the y-axis points to the left, and the z-axis points 
upwards. As the stereo camera and IMU rig is mounted upside-down facing right, we end up with 
the x-axis pointing right, the y-axis pointing backwards and the z-axis pointing down relative to 
our platform. To transform this frame to the body frame we simply have to perform a 270° rotation 
along the z-axis. 

The frame for the cameras have the x-axis pointing to the right in the captured image, the 
y-axis pointing down, and the z-axis pointing “into” the image, i.e. forwards from the camera. As 
again the cameras are mounted upside-down facing the right, the x-axis will point forwards, the 
y-axis upwards and the z-axis to the right relative to our platform. To transform this frame to the 
body frame we perform a 90° rotation along the x-axis. 

As for the GNSS receiver, we are only interested in how the heading measurement is trans- 
formed. Since the z-axis points downwards in both the GNSS and body frame we do not need to 
transform this. 

So far we have not mentioned the translation between the origin of the different coordinate 
frames. As we are interested in the heading estimation in this project, and since the IMU and 
camera are measuring delta-angles, and the GNSS heading vector cuts through the body frame 
origin, compensating for the lever arms will not have any noticeable effect on our system. 
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3.3.2 Mounting Errors 


Since we are not able to mount all our equipment perfectly to the testing platform, there are bound 
to be some errors that could affect our measurements. To measure these errors we assume that 
the right side of our platform is perfectly straight. We also assume that our antennas are identical 
in size, and ignore any production errors. 

Figure 3.8 shows the measurements we have taken of our testing platform after mounting all 
the equipment. Here, the rectangles with circles in them represents our antennas, and the red lines 
at each end represent our iron sight. The measurements are given in millimetres. 
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Figure 3.8: Mounting errors for our testing platform (position errors are in millimetres). 


We start by looking at the error that will arise at the different baseline lengths due to the 
error in mounting the antennas. From Figure 3.8 we can see how far over the right side edge the 
antennas are sticking out, and we use this to determine the angular error we will experience. 

For the longest baseline of 1.455 m, we have a difference in antenna position of 0.3 mm. The 
angular error therefore becomes: 


0.7 — 0.4 


Orongbaseline = tan”? (Su 


) — 0.012* (3.4) 
As for the medium baseline of 1 m, the antenna position difference is 0.8 mm. The error 
becomes: 


1.2— 0.4 


B ottiene LR" ( 
Finally for the short baseline of 0.5 m, the ratio of antenna position difference and baseline 
length stays the same. Thus the error will be equivalent: 


0.8 — 0.4 


O smalbasetine = tan"! 
500 


) = 0.046 (3.6) 


3.3. COMPLETE SYSTEM 21 


We also have errors in the positioning of the iron sight we have mounted to aid in the aiming of 
the platform, that will affect the heading measurement. From Figure 3.8 we see that the difference 
in positioning between the front and rear sights is -0.07 mm. With a distance between the sights 
of 1.273 m, the error becomes: 


.06 — 6.1 
bscope = tan! (* = >) = —0.003° (3.7) 





As this error is very small (aiming at a position 2 km away will give an error of 0.1 m), it will 
be ignored here. 
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Chapter 4 


Sensor Models 


This chapter describes the mathematical models of our sensors we have implemented in NavLab. 
It looks at the error models, and how we get the information in our systems on the state-space 
form. 


4.1 IMU and GNSS 


The IMU and GNSS position classes are already implemented in NavLab, so we only need to find 
the correct sensor parameters. As for the GNSS heading measurement, we will be using a modified 
compass class. The error state in all of these classes are modelled as 1. order Markov processes, 
as explained in Section 2.5. Our goal then becomes to find the standard deviation of the bias 
Orias, the standard deviation of the white measurement noise o,, and the time constant T, for each 
sensor. 


IMU 


From section 3.1.2 we see that we have most of the values we need listed from the IMU’s user 
manual and calibration certificate. Some slight modification is needed to get them to the unit that 
NavLab requires. 

As the parameters for the gyroscope are all listed in degrees, while NavLab requires radians, 
we must do a quick conversion for these: 


O rine gyro = 005^ /5 » —  — 0.000875 rad/s (4.1) 
| 180° 
s gyro = 0.015" /5- ¿go = 0.000263 rad/s (4.2) 


For the accelerometers, the measurement noise is given in ug/v Hz while NavLab needs m/s*/?. 
The bias is given in the correct unit. The parameters therefore becomes: 


Orias,ace = 0.03 m/s? (4.3) 


O: aco = 80 ug/ V Hz =80-10"* - 9.81m/s*” = 0.000785 m/s?” (4.4) 


As for the time constant, experience working with this type of IMU at FFI suggests that 
T = 600 s for both the accelerometer and gyroscope measurements is a fair assumption. 


GNSS 


The GNSS receiver outputs the standard deviation of the actual measurements. We can therefore 
use these directly in NavLab, using the variable quality functionality. Before we can do this, we 
have to determine how much of the noise is coloured, and how much is white noise. 

Experience with GNSS at FFI suggests that around 80% of the noise will be coloured, and 
around 20% will be white. We therefore multiply the standard deviation reported by the receiver 
with 0.8 and 0.2 respectively, and send these values into NavLab. 
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Experience also suggests that a time constant of 60 seconds is a fair assumption for the GNSS 
measurements. 


4.2 Camera 


As there is no camera class available in NavLab for our camera sensor, we have to model this com- 
pletely. As previously stated, we are primarily interested in maintaining the heading of our system 
as accurate as possible. We will therefore investigate the angle measurements from the camera 
sensor. The values from camera measurements here have the subscript VO (Visual Odometry). A 
tilde above a value indicates a measured value, while a hat indicates a calculated value. 


4.2.1 Error Model 


From our visual navigation algorithm, we receive a measurement of the rotation of the camera 
from one frame to the next. We call this the delta rotation matrix between the body frame at time 
step 1, and the body frame at time step 2: Ry, »,. AS we saw in Section 2.3, this rotation matrix 
can be uniquely defined by three angles, which we gather in the vector 05, s,- 

If we now assume that we have a high rate of measurements from the camera, and small 
rotations between the frames, we can approximate this vector as as the angular velocity between 
the body and local frame, by dividing with the time step. 





0 
~B ~ B1B2 
Wir vo "d At (4.5) 
This measurement will consist of the true value wfs vo, plus some error dWw%7 vo that we now 
have to model. 
~B B B 
Wir vo m Wir vo zs OUT vo (4.6) 


During testing our camera has to look at the same scene over multiple time steps to measure its 
movement. We can therefore assume that the measurement will have a bias Aw7, vo, plus some 
white measurement noise yo. 


Or svo E PT. vo + Evo (4.7) 


This bias will be modelled as a 1. order Markov process driven by normal-distributed white 
noise: 











. 1 
Aw? a vo iex Awis vo + Yvo (4.8) 
vo 
Where Tyo is the time-constant, and: 
1 
— 0 0 
i Tvo,z i 
AL 0 = 0 4.9 
Tvo Tvo,, i (69) 
0 0 — 
Tyo, 


The subscripts x, y and z here denotes the x-, y- and z-axis. Yvo is the white noise which 
drives the bias, called the process noise, and 


Yvo, 
Yvo = | Woy (4.10) 


Yvo,z 


This white noise vector is normal-distributed, so we can write Yvo ~ N (0, Qvoô(T)), where 
Qvo is the spectral density matrix. For now we assume the bias errors along the three axis are 
uncorrelated, and can write: 


A 0 0 

PU vov 

Qvo = 0 Tavos 0 (4.11) 
0 0 Ge 
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Future work should include investigating if there exists correlated errors. To find aĝ o, we 


can look at the variance of a 1. order Markov process, by inserting equation 4.8 into the predicted 
estimate covariance in equation 2.31: 


- Deo a E 
P = -— P + Qvo (4.12) 
Tvo 
This equation has the solution: 
= t—t > 2 T = TÍA (tt 
P(t) - e "vo" p, + Svolveg —e mau) (4.13) 
The steady state solution (t > oo) becomes: 
£ IoT, 
P. = Svotvo (4.14) 
We therefore get: 
BAGNO : 
SA = — (4.15) 


As for the white measurement noise, we assume that is is the sum of a large number of small 
random components, such that it has a Gaussian distribution. We therefore assume that yo, ~ 
N (0, W,o.,0,,), where Wyo., is the covariance matrix. If we again assume uncorrelated errors 
between our axes, this becomes: 


: 0 0 
VO,k>»T 
Wrox. = 0 obs 0 (4.16) 
0 0 oÈ, 
TiVO, k” 


We have now completely modelled the bias and white noise errors. What is left is to determine 
the parameters Urovomr levo and Tyo. 

To find the noise parameters Ta. 0 pias and Ceyo, we can look at the degree per pixel resolution 
in our image, and assume that our standard deviation is 1 pixel. From section 3.1.3 we found that 
the FOV in our images is about 98.25? in both x and y direction. With a resolution of 1200x1200 
pixels in our image, this equals: 


_ 98.25° 
Prot = 1900p 


We can assume that our error is slightly larger than this, due to other errors in the visual 
navigation algorithm like errors in the camera model, errors in 3D reconstruction, errors in motion 
estimation etc. We therefore assume a total standard deviation of 0.1°. After some tuning using 
our testing data, it seems splitting this error evenly into coloured and white noise yields the best 
result. Finding the time-constant is slightly trickier, but tuning again gives a time constant of 60 
seconds as a good result. We therefore end up with: 


= 0.082° /p (4.17) 


= 0.05°/s, ogo =0.05°/s, Tyo = 60 5 (4.18) 


Ü ^vvo bias 
Note that the tuning carried out to find these parameters was done on a very small data set, so 


the parameters might be finely tuned to these specific sets of data. For future use, a more thorough 
investigation should be done to see if these parameters hold for other data sets. 


4.2.2 Error State 


As NavLab uses an ESKF, we need to find a redundant version of our measurement to calculate 
our error state. As is we do not have this version, but we can make one using other known 
measurements and calculations: 


Ors = Ri pO, = Riese, = OL. (4.19) 
Here G7, is the gyro measurement from the IMU, c7, is the known angular velocity of the 


earth equal to [7.29 - 107*,0,0]%, and @%, is found using the navigation equation A.2. 
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By multiplying R,» with our measurement Os. vo; and subtracting this from our new redun- 


dant state, we get our error state: 


Yy = is — Ris (4.20) 


LB,VO 


4.2.3 State-Space Formulation 


To add our new sensor to the ESKF in NavLab, we need to set up all the system information on 
the continuous-discrete state-space form, as shown in Equations 2.28 and 2.29. As our continuous 
process model is discretized by NavLab, we do not account for that process here. 

We first look at the camera class part of the continuous process model. This is simply the 1. 
order Markov process we found in equation 4.8: 


. 1 
Aw? — Awis vo + Yvo (4.21) 


LB,VO T 
vo 


We then have the camera class part of the continuous process model in equation 2.28, and have 
that: 


1 
F=—-——, L=0, G=1 4.22 
Tyo ( ) 
and 
z= Aw ip vó u — 0, Y= Yvo (4.23) 


The spectral density matrix of y is given in equation 4.11. 

We will now find the discrete measurement model. Our base is the error state we found in 
equation 4.20. By replacing the measured and calculated values with the true value plus the error, 
we get: 





y = (Ris + Rrs)(wrs + Swie) — (Riz +8Rre)(wWrin + bwin) — (We, +wp) 


(4.24) 
—(Riz +0R ¿5 MW, + Aws vo + Evo) 
First order approximation gives: 
y =R,¿pwÍ, — Rig, — wg, — Rip, +R pw, + R¿p0w7, — OR ¿ws (4.25) 
—R,zów, zu wz, Po OR sors RAW» vo Rivkvo l 
We can now remove the terms without errors in them since these are equal: 
y -óR, so, + R¿pg0w;, — OR, pw, — R¿n0w;, — Óówr, (4.26) 
—óR, swis = Ri 2Aw? s vo = Rizébvo 
By using 2.16 we get: 
y = S(er,) R,swps + Rindw;y — Sep) Rie), — Rindw;y — wr, (4.27) 
—S(er,)Rusw7, — RrsAw?, yo — Rintvo l 
Or: 
y= S(er,)uwrs T R, sôw7s ES S(er,)wrs = OW rs = OW rr (4 28) 
—e(er ur. T R,,Aw?, vo xS Riskvo l 


By using A.6 and A.8 from the error propagation of the navigation equations in appendix A, 
we get: 
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1 
=S eL wrs + R: sôwrs m he Wi +5 Car Wip =e Urs OVE5 
y = S(er,) (ers) (ez) m (uz) (4.29) 
e ee eH RAW» vo m Rrsvo 


Or: 


1 
y= S(er 5) (rs z wis) T PTT E + 25 (51) rp - — S(u;,)óvz,— 


TEB (4.30) 
Ra -Ringvo 
We can also make the following simplification for wf, — wr: 
Win = wis = Rp, ES Ri pw; , F Ry pwr, AD Wer = Wie 2» Wer (4.31) 


We then finally end up with: 


1 
y=|-S(wt, +w) ———S(ubj) —-25(wt,) Ris 0 -Ris x + [-R.s£vo] (4.32) 


EB 


Here, the state vector x have been augmented with the error states from the navigation equa- 
tions and the IMU class, and is equal to: 


r= EL (4.33) 


We then have the camera class part of the discrete measurement model in Equation 2.29, and 
have that: 





AU ai) 28 (wis x) Rss 0 —Rimxl, 
fun (4.34) 


Er = — En aÉvo, 


The covariance matrix for €, is now altered from Equation 4.16, since we are multiplying with 
Rs: 
W, = cov(É,) = Ris pévo.(Rre von) = Ris Wvo Per, (4.35) 


We now have the complete system model on the state space form, and can write this into a 
camera class in NavLab, that allows us to add measurements from the camera to the ESKF. 
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Chapter 5 


Results 


This chapter describes the different tests performed with our platform, and shows the results from 
these tests. In total, three types of tests were performed. In the first test we addressed how well 
the GNSS receiver performs with different antenna baselines. The second test is to find how the 
GNSS receiver responds to being jammed. Finally, in the last test we run the complete system to 
see how well our camera can aid the IMU in estimating the heading when the GNSS signal is not 
available. 


All our tests were performed on the roof of FFI. We aimed our platform at Roverkollen telecen- 
tre, which is a 84 meter high telecommunication tower located north-east of Oslo. As we can see 
in Figure 5.1, the tower is located 8424.27 meters from our testing point, with an angle of 273.58° 
relative to north. This angle was also confirmed using the azimuth function in Matlab’s Mapping 
Toolbox. 
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Figure 5.1: Map of testing area. 


Figure 5.2 shows the testing platform aiming at the tower. The tower might be difficult to see 
in the picture due to foliage, but it provided a clear point for us to aim at during the tests. 
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Reverkollen Tower 
É 2 








Figure 5.2: Aiming at Roverkollen telecentre. 


5.1 GNSS Receiver Baseline Test 


During this test, our goal is to see how accurate the GNSS receiver can measure heading, with 
different antenna baseline lengths. As previously mentioned, the practical maximum baseline 
length we get between our antennas on a standard assault rifle is about 0.5 meters. If our tests 
indicate that this baseline does not provide sufficient accuracy, an idea is to mount some sort of 
extension to the rifle that will give us a longer baseline. For this reason we have also tested the 
GNSS with a few longer baselines. As we saw in Section 3.3, we have made mounting holes for our 
antennas at four locations on the platform, giving us the following possible baseline lengths: 


1. The “long” baseline is 1.445 meters long 
2. The “medium” baseline is 1 meter long 


3. The “small” baseline is 0.5 meters long 


For each baseline length we completed 9 tests, each of 1 minute duration, logging heading with 
a frequency of 1 Hz, and powering on/off the GNSS receiver between each 1 minute test. During 
all tests the platform was directed at Roverkollen telecentre. The following sections shows the 
results from these tests. 


Long baseline 


In the long baseline test we set up our antennas at the ends of the testing platform, giving us a 
total baseline of 1.455 meters. Figure 5.3 shows the results from the first of the 9 tests done at 
this baseline length, while the rest of the tests are summed in Table 5.1. This table shows the 
mean heading for each test, and its standard deviation. It also shows the error between the mean 
heading and the true heading. The table also includes one run were we have calculated the total 
mean and standard deviation using all the samples in all the runs. 

From Section 3.3.2 we saw that our error in mounting the antennas will cause an angular error 
of 0.012° for this baseline length. The true heading for this test is therefore 273.59°. 
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Figure 5.3: Long baseline test 1, showing the heading measurement series, with its mean and 


standard deviation. 





























Run Mean heading | Standard deviation | Error from true heading 
i 273.3399 0.0457 -0.2501 
2: 273.2358 0.0354 -0.3522 
3. 273.2998 0.0457 -0.2902 
4. 273.4188 0.0909 -0.1712 
5. 273.4519 0.0818 -0.1381 
6. 273.3052 0.0689 -0.2848 
7. 273.4141 0.0640 -0.1759 
8. 273.3897 0.0432 -0.2003 
9. 273.2711 0.0672 -0.3189 

All samples 273.3297 0.1049 -0.3003 








Table 5.1: Results from long baseline test, showing the mean and standard deviation of each test, 
and the difference from the mean to the true heading. 


Medium Baseline 


In the medium baseline test we move the front antenna to the second to last mounting spot, leaving 
the reference antenna at the same position, and giving us a total baseline of 1 meter. Figure 5.4 
shows the results from the first of the 9 tests done at this baseline length, while the rest of the 
tests are summed up in Table 5.2. 

Section 3.3.2 shows that our error in mounting the antennas will cause an angular error of 
0.046° for this baseline length. The true heading for this test is therefore 273.63°. 
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GNSS Medium Baseline Test 1 
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Figure 5.4: Medium baseline test 1, showing the heading measurement series, with its mean and 


standard deviation. 


























Run Mean heading | Standard deviation | Error from true heading 
1. 273.4266 0.1270 -0.2034 
2. 273.3804 0.0907 -0.2496 
3. 273.5237 0.1413 -0.1063 
4. 273.6558 0.1382 0.0258 
5. 273.3412 0.1058 -0.2888 
6. 273.2285 0.0999 -0.4015 
Ts 273.4961 0.1297 -0.1339 
8. 273.2977 0.1152 -0.3323 
9. 273.3303 0.1032 -0.2997 

All samples 273.3747 0.1951 -0.2553 











Table 5.2: Results from medium baseline test, showing the mean and standard deviation of each 
test, and the difference from the mean to the true heading. 
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Small Baseline 


In the small baseline test we move the front antenna again to the position closest to the back 
antenna, giving us a baseline of 0.5 meters. Figure 5.5 shows the results from the first of the 9 
tests done at this baseline length, while the rest of the tests are summed up in Table 5.3. 

From section 3.3.2 we saw that the angular error caused by the error in mounting the antennas 
remained the same as for the medium baseline. The true heading for this test is therefore the same 
as from the medium baseline test, 273.63". 
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Figure 5.5: Small baseline test 1, showing the heading measurement series, with its mean and 
standard deviation. 


























Run Mean heading | Standard deviation | Error from true heading 
1. 272.7999 0.2144 -0.8301 
2. 273.0067 0.1572 -0.6233 
3. 273.0572 0.1653 -0.5728 
4. 273.0331 0.4312 -0.5969 
5. 273.1841 0.1502 -0.4459 
6. 272.9076 0.2111 -0.7224 
T. 213.0679 0.1845 -0.5621 
8. 272.8558 0.2102 -0.7742 
9. 273.0425 0.1884 -0.5875 

All samples 272.9381 0.2533 -0.6919 








Table 5.3: Results from small baseline test, showing the mean and standard deviation of each test, 
and the difference from the mean to the true heading. 





32 CHAPTER 5. RESULTS 


5.2 GNSS Receiver Jamming Test 


In this test we tried to jam the GNSS receiver on our platform to see how it reacted. This was 
done by using a very low power GPS-jammer on the L1-frequency (1575.42 MHz), with a 20 MHz 
bandwidth. 

Two tests were conducted, one with the short baseline between antennas, see Figure 5.6, and 
one with the long baseline between antennas, see Figure 5.7. 
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Figure 5.6: Jamming test 1, showing the estimated heading and its standard deviation from the 
GNSS receiver, and the amount of satellites used in the solution. The jamming starts at 10 seconds. 
The small baseline between the antennas is used. 
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Figure 5.7: Jamming test 2, showing the estimated heading and its standard deviation from the 
GNSS receiver, and the amount of satellites used in the solution. The jamming starts at 10 seconds. 
The long baseline between the antennas is used. 


As seen in both Figure 5.6 and 5.7, as soon as the jamming is started, the heading measurement 
immediately drops to zero. This could pose a problem if we only look at the measurement itself, but 
as we can see, the number of satellites used in the solution also drops to zero. The solution status 
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parameter in the log is also changed from SOL_COMPUTED to INSUFFICIENT_OBS. If we monitor these 
parameters, and discard any measurement with the incorrect status, jamming should not cause us 
to receive faulty measurements. 

As the heading log only updates on change, no measurements are received until the jamming 
is stopped. Once resumed, we see that the GNSS receiver needs a few seconds to acquire enough 
satellites to get a correct heading measurement. We can see how the standard deviation of the 
heading measurement given by the GNSS receiver reflect how many satellites it uses in the solution. 


5.3 Visual Navigation Test 


The tests of the visual navigation uses our IMU and the stereo camera rig to see how well we can 
keep our heading when we lose the GNSS signal. In total four tests were completed, with a varied 
amount of movement of the platform in each one. The initial heading is first established using 
measurements from the GNSS receiver, but are then disabled once the camera data starts. The 
estimated heading is presented for each test both when the IMU is acting alone, and when the 
camera is assisting in the estimation. 

Unfortunately, because of the long setup time and the limited availability of the stereo cameras, 
no further tests were completed. The small amount of test data we acquired does provide some 
problems when we try to extract any statistical information from them, as we will see in Chapter 
6. 

Since the extraction of the images takes a very long time, these tests were run with the camera 
providing data at the rate of 10 Hz. One of the tests has also been run at 70 Hz to check if this 
has any effect on the result. 

During analysis of the data, it was discovered that the IMU causes so much drift in position 
that it has an adverse effect on the estimated orientation. Because of this, the GNSS position data 
has been used through the entire test, even when we assume we lost the GNSS signal. We did this 
to get a more realistic result from our tests, since in the final product the camera would also be 
used to estimate the translational motion of the system, thereby reducing the translational drift 
from the IMU. 
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Figure 5.8: Visual navigation test 1, showing the estimated heading with both the IMU alone, 
and the IMU + camera providing data. The period where the camera data is available is from 
t = 264.3 to t = 378.8. 
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Test 1 


This test was done while the platform was standing completely still. The data in this test was 
mainly taken during the setup phase, so the cameras did not start recording until quite late in the 
test. Figure 5.8 shows the estimated heading for the case when only the IMU data is active, and 
the case when both the IMU and camera are providing data. As seen in the figure, the estimated 
heading at the start and stop points for the camera data are highlighted. The period where the 
camera data is available is from t = 264.3 to t = 378.8. Until this point we use the GNSS heading 
data, which is why the estimated heading is so erratic. 

From Figure 5.8 we can see that with only the IMU estimating the heading, a heading error of 
0.3 is accumulated over 114.5 seconds. When we include the camera data in the estimation, this 
error drops to 0.1”. 


Test 2 


This test was completed with the platform first standing still, then slowly rotated back and forth 
around the z-axis to create disturbances in the heading. The platform was then re-positioned to 
its original heading and left stationary until the end. Figure 5.9 shows the estimated heading with 
both the IMU acting alone, and the camera assisting in the heading estimation. The period where 
the camera data is available is from t = 25.7 to t = 133.9. 

During this test, the feet of the tripod our platform is using were not completely secure, causing 
the entire platform to drop slightly in the back. This made it difficult to aim it correctly after its 
movement, so the final heading could be slightly wrong because of this. 
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Figure 5.9: Visual navigation test 2, showing the estimated heading with both the IMU alone, and 
the IMU + camera providing data. The period where the camera data is available is from t = 25.7 
to t = 133.9. 


Figure 5.9 shows an accumulated heading error of 1.0° over 108.2 seconds when only the IMU 
is estimating heading. Including the camera data into this estimation improves the error to 0.2°. 
Test 3 


Since we experienced some problems with re-aiming our platform at the end of test 2, another test 
was taken with the same slow rotation in yaw. Figure 5.10 shows the resulting heading when both 
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the IMU is acting alone, and when the camera is assisting in the estimation. The period where 
the camera data is available is from t = 11.9 to t = 140.7. 

In this test we also extracted the images from the camera at 70Hz, to see if this would have 
any effect on the estimation. 
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Figure 5.10: Visual navigation test 3, showing the estimated heading with both the IMU alone, 
and the IMU + camera providing data. The period where the camera data is available is from 
t — 11.9 to t = 140.7. 


From Figure 5.10 we can see that with only the IMU estimating the heading, a heading error of 
1.1? is accumulated over 128.8 seconds. When we include the camera data in the estimation, this 
error drops to 0.2”. In addition, Figure 5.10 shows us that there is no improvement in the heading 
estimate when running the camera data at 70Hz. This could be because of our slow rotation of 
the platform, allowing the camera to easily detect several feature points over multiple time steps. 
In hindsight, we should have run the camera data at 70Hz in test 4, were we have a much faster 
yaw rotation. 


Test 4 


In the final test we carried out the same movements as in test 2 and 3, but this time at a much 
higher speed. Figure 5.11 shows the estimated heading for the case when only the IMU data is 
active, and for the case when both the IMU and camera are providing data. The period where the 
camera data is available is from t = 13.6 to t = 99.6. 

Figure 5.11 shows an accumulated heading error of 0.6” over 86.0 seconds when only the IMU 
is estimating heading. Including the camera data into the estimation improves the error to 0.3". 
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Visual Navigation Test 4 
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Figure 5.11: Visual navigation test 4, showing the estimated heading with both the IMU alone, 
and the IMU + camera providing data. The period where the camera data is available is from 
t = 13.6 to t = 99.6. 
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Chapter 6 


Discussion 


In this chapter the results from the different tests are discussed, and suggestions to further work 
is provided. 


6.1 Dual Antenna GNSS Performance 


The main goal for the dual antenna GNSS test was to see how the heading estimate was related 
to different baseline lengths. This was because specifications for the GNSS receiver only state 
heading accuracy with a baseline of 2.0 meters, while the maximum length on a standard assault 
rifle is about 0.5 meters. The receiver was therefore tested with a baseline length of 0.5 meters, 
but also with baseline lengths of 1.445 and 1.0 meters, in case the smallest baseline did not provide 
a sufficient estimate. The specifications of the GNSS receiver are also presumed to be based on 
ideal conditions, while in the real world we may for example have reflecting surfaces that create 
multipath disturbances. 

We would assume that the shorter baseline we test with, the lower the accuracy and precision 
will get. Accuracy is a measure of how far the mean of a set of samples is from the true value, 
and precision is a measure of the distribution of those samples. In Figure 6.1 we have fitted a 
normal distribution to the total set of samples for each baseline length, to illustrate the accuracy 
and precision results from the different tests. 
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Figure 6.1: Normal distributions fitted to the total amount of samples from each baseline test, to 
illustrate the difference in precision and accuracy. 


As we can see from Figure 6.1, the precision of the samples drop as we lower the baseline length, 
as assumed. However we can see that the accuracy of the medium baseline samples are better than 
the large one. This result could be explained by slight fluctuations in our aiming accuracy, or 
changes in the satellite constellation (e.g. satellites no longer tracked/new satellites in view). It 
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should also be mentioned that we have calculated the accuracy based on only one measurement 
series for each baseline, which is too little information for us to draw precise conclusions. More 
test series should be conducted to determine this relationship. 

The small baseline is the most interesting to look at, since this is about the baseline length we 
can expect on a standard assault rifle. Even though the result is worse than from the medium and 
large baseline, the error for the small baseline stays well within 1°. We will look at how this error 
affects the total target localization error later in this chapter. 


6.2 Heading Estimation With IMU and Camera 


Since the reliability of GNSS signals on the modern battlefield are questionable because of the 
ease of jamming these, our system has redundant sensors in place in the form of an IMU and 
camera to maintain the accuracy of the estimated heading when GNSS is not available. The IMU 
and camera data has been fused using NavLab to create a heading estimate. The four tests we 
completed, described in Section 5.3, are summed up in Table 6.1, showing the heading drift with 
and without camera data, and the time period from the beginning to the end of the run. The table 
also shows the movement in yaw carried out during the test. 




















Test number | Only IMU | IMU and camera | Time | Movement in yaw 
1 0.3° 0.1* 114.5 s None 
2 1.0? 0.22 108.2 s Slow 
3 -1.1° -0.2° 128.8 s Slow 
4 0.6° -0.3° 86.0 s Fast 























Table 6.1: Results from the visual navigation tests, showing drift in heading estimate with and 
without camera data. 


6.2.1 Only IMU 


The second column in Table 6.1 shows heading drift of the IMU. This drift in yaw happens due 
to integration of biased and noisy gyroscope data, compared to the roll and pitch that in a static 
situation is calculated using the gravitational vector directly. As we can see, heading drift is about 
+1” in time period of about 2 minutes. This drift is quite significant, and will cause a position 
error of +35 meters at a range of 2000 meters. If the drift keeps increasing at this rate, the 
estimated heading will soon be completely useless for our cause. It is therefore clear that we need 
an additional sensor to support the IMU and reduce this drift. For this reason we have added 
visual navigation data from our cameras. 








6.2.2 Adding Camera Data 


The third column in Table 6.1 shows the drift in heading when combining the IMU and camera 
data. As we can see, the drift is now significantly reduced for all of our tests, with around +0.2° 
drift in a time period of about 2 minutes. The drift is still present due to errors in the visual 
navigation algorithm, e.g. the pixel resolution error. If we compare our result now from when we 
only used the IMU data, our new drift of +0.2° will cause a position error of only +7 meters at a 
range of 2000 meters. 

Table 6.1 also shows that test 1 had no movement of the platform, test 2 and 3 had slow 
movement in yaw, and test 4 had rapid movement in yaw. It seems then from looking at the 
resulting drift in Table 6.1 that the more rapid movement our system experiences, the worse the 
camera is able to compensate for the drift. It is difficult to draw any firm conclusions from the 
test results, since we have a limited amount of samples available. 

In this estimation we have not taken advantage of the loop closing feature of ORB-SLAM2, i.e. 
that it can correct for drift when the camera returns to a scene it has seen before. An idea could 
be to have a method of switching between this feature when the soldier is stationary, and turning 
it off when he is in motion to save processing power and storage space. This would reduce the 
total accumulated drift, since the estimation would only drift when the soldier is in motion. This 
would only work with SLAM algorithms though, and not pure visual odometry algorithms. 
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6.3 Total Target Localization Error 


One of the system requirements is that the total target localization error should not exceed 30 m 
circular error probable (CEP). CEP is a measure of the spread in the horizontal plane, and is equal 
to a circle with radius rj, one will hit within with 50% probability. The relationship between the 
CEP and the standard deviation is given in [23]: 


CEP = r4, = \V/—2In(1 — 1/2) o (6.1) 


The CEP assumes, as the name suggests, that the error in the horizontal plane is circular. This 
means that the uncertainty in side and length must be equal, 0. +. = Cy tot: If the uncertainty 
in side and length is approximately equal, Ostot Y 9,,4,, we can find an approximate standard 
deviation that is equal in side and length [23]: 


o? = Elola + OBa) (6.2) 

For us this will be the case only at one point, since our heading error will cause a side error 
that will change depending on the distance to the target, while the error in length, measured by 
a laser rangefinder, will stay constant. Since our requirement is using CEP, we will approximate 
a circular error using Equation 6.2, although this is not ideal since our error in general will be 
elliptical. 

We will be looking at the total target localization error in the case were we have the smallest 
baseline, since this is the most realistic baseline length. From section 5.1 we found that all our 
measurements over all our tests with the smallest baseline had a standard deviation o, of 0.2533°, 
with an error from true heading, or bias, of -0.6919*. This bias seems quite consistent for all our 
tests, so we can assume that this is an error we can remove with calibration (e.g. errors due to the 
antenna phase centres). In the future, to confirm if this bias is something we can calibrate, some 
tests should be run pointing at different headings to see if this bias still stays constant. 

'The standard deviation of the heading measurement now has to be transformed into a standard 
deviation for the side uncertainty. To do this we use Equation 6.3, which gives us the uncertainty 
in arc length. At small angles this is approximately equal to the side uncertainty. 

T 
18057 

Here d is the distance from our position to the target. 

For c, we use the standard deviation from a laser rangefinder that FFI has previously tested 
in target localization, which gave a value of 1.44 meters [23]. We also need to consider the error in 
our own horizontal position. The specifications of our GNSS receiver states a horizontal accuracy 
of 1.2 meters. Experience with testing civilian GNSS receivers at FFI shows that this is a bit of 
an optimistic estimate, and suggests that a standard deviation of 3 meters is a more conservative 
estimate. We will therefore be using o,,, = 3 m. This standard deviation now has to be added to 
our standard deviations in both side and length: 


Or, tot = Y o? + Cans; Oy tot = y 02 + Dos (6.4) 


We are now able to calculate the systems CEP, as long as the GNSS is able to provide heading 
data. In the case were we loose the GNSS signal, we are interested in how the CEP changes when 
the heading estimate has drifted over a period of time. Getting any statistical info from our data 
in Table 6.1 on how the IMU and camera cause drift is very difficult, since we only have 4 tests, 
each with different baseline length and different motion applied. We will therefore do a very rough 
estimate to find an approximate standard deviation, by using the worst case scenario for both the 
IMU drift and the IMU + camera drift. From Table 6.1 we see that this is the case for the drift 
in IMU in test 2, 1.0? in 108.2 seconds, and for the IMU + camera drift in test 4, 0.3 in 86.0 
seconds. We will also make another assumption which is that the drift is linear over short times, 
which allows us to see how the drift affects the heading measurement over a longer period of time 
than our test lengths. If we now integrate the worst case drift from test 2 for the IMU and test 4 
for the IMU + camera up to 5 minutes, and assume that this drift is a standard deviation, we get 
Ocam = 1.0453° and Cimu = 2.7726°. Here the subscript cam indicates the IMU + camera drift, 
and the subscript imu indicates the IMU drift. Note that we do this very rough estimate of the 
standard deviations, since we have a very low amount of tests done over a very short time period. 


(6.3) 


o,=d 
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In future work, more tests should be completed over a longer time period to properly determine 
these values. 

We now sum the standard deviations we determined for the IMU and IMU + camera drift with 
the standard deviation from the GNSS receiver to get a new standard deviation for the heading in 
both cases: 


Oo, cam = V 0$ + ams O0 imu = V o3 M OI is (6.5) 


Inserting equations 6.5, and the standard deviation for the GNSS measurement c, = 0.2533°, 
into Equation 6.3, gives us the side uncertainty for the GNSS measurement, IMU drift and IMU 
+ camera drift. Inserting these values, the uncertainty from position measurement and the un- 
certainty in length into Equations 6.4 gives us the total uncertainty in side and length. Using 
Equation 6.2 we calculate the approximate circular error, and finally insert this into Equation 6.1 
to get the CEP. The CEP is plotted as a function of distance to the target in Figure 6.2. 
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Figure 6.2: CEP plotted as a function of distance to the target, using the small baseline GNSS 
heading data, and the drift accumulated after 5 minutes in the cases were IMU and IMU + camera 
is used to estimate the heading. 


We see from Figure 6.2 that when we use our GNSS receiver with a baseline between the 
antennas of 0.5 meters, we achieve a CEP of 30 meters or less up to a distance of 8087 meters, 
assuming that we have removed the bias during calibration, and after approximating the error as 
circular. 

Once the GNSS signal is disturbed, and the heading measurement is allowed to drift for 5 
minutes, we see that the distance we are able to achieve a CEP of 30 meters or less drops to 1904 
meters in the case were we use both the IMU and camera data, and to 735 meters for the case 
were only the IMU is estimating the heading. These results are assuming that we have removed 
the bias in the GNSS receiver during calibration, approximating the error as circular, can use the 
worst case drift from our tests as standard deviations, and that the drift is linear for small time 
periods. The result should therefore be taken with a grain of salt. Nevertheless, it does give us an 
indication we can use on how the drift will affect the heading estimate. As target localization in 
the field is mostly done up to a range of 2000 meters, based on our results we are able to achieve an 
acceptable CEP until about 5 minutes after loosing the GNSS signal, with an IMU and a camera 
estimating the heading, and keeping the assumptions we have done in mind. Figure 6.3 shows the 
same plot as in Figure 6.2, only zoomed in to the 0-2000 meter range. 
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Figure 6.3: CEP plotted as a function of distance to the target, using the small baseline GNSS 
heading data, and the drift accumulated after 5 minutes in the cases were IMU and IMU + camera 
is used to estimate the heading. The range to target is limited to 2000 meters. 


6.4 Further Work 


As the final result of this project is a proof-of-concept version, a lot more work is needed before 
the final product is ready. What follows are some suggestions of further work that can be done. 


e Running longer and more tests: The tests we performed to see how well we could keep our 
estimated heading without GNSS turned out to be quite short. It is therefore useful to run 
some longer test to properly determine how well the heading is kept after a long time. It is 
also needed to run tests aimed at different headings, to see if the bias in the GNSS system 
can be calibrated. If this bias cannot be calibrated, it might be necessary to create some 
form of extension to the rifle to provide a longer baseline length for the antennas. More tests 
are also needed to properly determine the noise parameters of the camera. 


e Implementing camera position measurements: Only the angle measurements from the camera 
has been implemented in NavLab in this thesis. A natural step further would be to implement 
the position measurements, and to include translational motion during testing. 


e Further reduction of the heading drift: From our results we saw that the heading estimate 
still has a drift after combining the IMU and camera data. This drift could be further reduced 
as discussed using the loop closure feature of ORB-SLAM2. There are also a lot of recent 
research on the topic of reducing visual odometry drift, for example using different feature 
descriptors and feature transformations [5], inferring the sun direction [20] or estimating the 
vanishing directions [3]. 


e Improving the camera error model: In our error model of the camera we have only directly 
modelled the drift caused by the pixel resolution error. The visual navigation algorithms used 
could also accumulate other errors, like errors in the camera model, errors in 3D reconstruc- 
tion, errors in motion estimation etc. To improve the heading estimation, these errors must 
also be investigated. It should also be investigated if there exist some correlation between 
the errors on the three axis. 
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Increasing the mobility of the platform: The sensors mounted on the current testing platform 
are not built to be very mobile, since they require power and connected computers. Further 
work on this project should include the setup of batteries and smaller data-loggers so the 
testing platform can perform more realistic tests in the field. 


Using smaller components: The components we have chosen in this project are probably too 
large to fit on a standard assault rifle. It is therefore necessary to look at reducing the size 
of these. Especially the GNSS antennas are quite large, so smaller versions of these should 
be procured. It is also possible to drop the enclosure for both the IMU and GNSS receiver 
to create a more integrated system. 


Investigating other means of finding heading: Once the testing platform is more mobile, 
it is possible to investigate if some of the other means of finding heading could be more 
accurate, for example measuring the velocity or acceleration vectors. It could also be useful 
to implement the methods where recognisable objects are used, to have a redundant way of 
finding the heading when this is possible. 


Communication: In this thesis it has not been discussed how to distribute target coordinates 
to fire control teams once target coordinates are determined. This is another point that needs 
to be addressed for a final system. 
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Conclusion 


The main goal of the thesis was to create a proof-of-concept version of a compact sensor system 
able to provide target localization functionality. The main objective has been to estimate heading 
from own position to the target, as this is the most difficult parameter to determine. 

A dual antenna GNSS receiver has been used to obtain system heading. Due to jamming of 
GNSS, which can be expected on the battlefield, our system has also been equipped with an IMU 
and a stereo camera in order to maintain estimated heading accuracy when the GNSS signal is 
lost. 

We have tested the GNSS receiver with a baseline length between the antennas that is much 
smaller than what is listed in the product specifications. This is because the realistic baseline 
length on a standard assault rifle is quite small. Our tests show that with this baseline length 
the precision of the heading estimate is still quite good, although a significant bias was observed. 
If this bias is something we can calibrate, the dual antenna GNSS receiver will work well for our 
purpose. If not, it might be necessary to create some extension on the rifle to be able to extend 
the baseline. 

Our testing also shows that using the stereo camera to aid the IMU in estimating the heading, 
provide a much better result than if the IMU was used alone. The camera still accumulates some 
drift, so after a period of time the estimated heading will not be accurate enough for our purpose. 
More work is therefore required to further reduce this drift. 

All of the goals we stated in the introduction has been completed: 


1. We have used the ROS framework in order to log and synchronize data from our GNSS 
receiver and IMU sensor. Although not used for the final tests, we have also tested and 
succeeded in logging pictures from a monocular camera in ROS as well. 


2. A testing platform has been constructed, that is able to mount all our sensors. There are 
also several mounting points for the antennas to be able to test different baseline lengths. 
The platform can be mounted on a tripod in order to keep the heading stable during testing. 


3. The data from our sensors has been fused using the error state Kalman filter in NavLab. A 
new class for the camera data has been written, since this did not exist prior to the start of 
the project. 


4. The dual antenna GNSS receiver has been tested at different baseline lengths to see how 
good heading estimate it can provide. The resulting data has been presented and discussed. 
A jamming test has also been performed, and how the receiver responded has been analysed. 


5. The complete system including the GNSS receiver, IMU and stereo camera has been tested, 
and the results have been presented and discussed. Because of limited amount of tests 
completed, it has been hard to draw any statistical data from them. More tests should 
therefore be done in the future. 


All in all the project has been successful, and performed according to the original plan. The 
results of this thesis will hopefully provide FFI with a good foundation for further research and 
work into this topic. 
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Appendix A 


Navigation Equations 


The navigation equations used in NavLab are well derived in [6], but the resulting equations are 
rendered here for convenience. 

The starting point for the navigation equations is the measurements from the IMU: f7, from 
the accelerometers and wP, from the gyroscopes. The end result will be the position given as Ry, 
the orientation of the system given as R5, and the velocity of the system given as vr, 

We start with the velocity, which when derived becomes: 


Dis = Risfis + 95 E (2075, zr Wer) x Uis (A.1) 


Where gi is the plumb bob gravity. By integrating A.1 we get v£,, but to do this we need 
Ris, wfs and wr, 


wi, = (lea X vo) (4.2) 
wr, = RygwL (A.3) 
Rip = R,sS(wP,) — S(wt, +wt,) Ris (A.4) 


Where rz, is the Earth radius plus the systems height above the Earth surface. uz, is equal 


to [0,0, —1]”. w*, is the known vector [7.29 - 107*, 0, 0]” rad/s. 


Finally Rz, is found: 


. 


Re, = Re S(w!,) (A.5) 


Since the measurements from the gyros and accelerometers have errors in them, it is also useful 
to see how these errors propagate through the navigation equations: 





uc x ER ROUaa] A.6) 

él, = OWE, + Eep, X WE A.7) 

bw), = en, X wh, A.8) 

êi, = Rew, — wip — bw, Fey X (Wi, Hwg) A.9) 


007, = Ris0f a + Els x fts T 99; ns (29; + Owes) x des = (2075, + 9i) x OVE 5 (A.10) 


Appendix B 


Code Listings 


Matlab Code 


% demonstrates stereo visual odometry on an image sequence 





disp (° D 
clear all close: all idbstop error: 





load (*fisheyeRectifyMaps.mat”); 


% parameter settings 


|; skipped frames = 7; 


last.frame = 6034; 


96 init visual odometry 
visualOdometryStereoMex( init’ , param) ; 


% init transformation matrix array 
Tr_total{1} = eye(4); 


% create figure 

Heare (Colo e 

hal = axes(’Position’ ,[0.05,0.7,0.5,0.5]) ; 
axis off; 

baog ases Posi trom) se OM OS (7589 07855 OnE 
set(gca, ’XTick’ ,—500:0.1:500) ; 

set(gca, ’YTick’ ,—500:0.1:500) ; 

axis equal, grid on, hold on; 


96 for all frames do 
for frame-first frame:last.frame/skipped.frames 


% 1—index 
k = frame—first_frame+l; 


% image index 
frameldx = framexskipped_frames; 


% read current images 
I1 = imread ([img-dir1 ’\’ num2str(frameIdx , ’%06d’) 
12 = imread ([img-dir2 ’\’ num2str(frameIdx , ’%06d’) 


% remap 
im.1 = cv.remap(I1,map1_0,map2_0); 
im.2 = cv.remap(I2 ,map1_1,map2_1) ; 


% equalize histograms 
im.1 = cv.equalizeHist (im_1); 
im.2 = cv. equalizeHist (im_2) ; 


%img-dir = '[home/geiger/5.Data/kitti/2011.stereo/2010.03.09.drive.0019 '; 
img-dirl = °E:\2017_05_03_taktest \Kamera_data\taktest6_extracted\image_0’; 
img-dir2 = 'E:42017.05.03.taktest \Kamera_data\taktest6_extracted\image_1’; 
param. f = 274.5918273014103; 

> param.cu = 616.3594910271584; 

3 param.cv = 635.2006218167855; 
param.base = —41.76118924363826/param. f; 
first_frame = 0; 


? 
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APPENDIX B. CODE LISTINGS 


% median filter 
im_1 = cv. medianBlur(im.1,”KSize” ,3) ; 
im.2 = cv.medianBlur(im.2, ’KSize’ ,3) ; 


% compute and accumulate egomotion 
Tr{k} = visualOdometryStereoMex(’ process’ ,im.1,im.2) ; 
if k>1 
Tr_total{k} = Tr_total{k—1}«*inv(Tr{k}); 
end 


% update image 

axes(hal); cla; 

imagesc(im.1); colormap (gray); 
axis oft: 


% update trajectory 
axes (ha2) ; 
if k>1 
plot ([Tr-total{k—1}(1,4) Tr-total{k}(1,4)], ... 
[Tr _total{k—1}(3,4) Tr_total{k}(3,4)], ’—xb’, ’LineWidth’ ,1) ; 


[roll(k), pitch(k), yaw(k)] = R2xyz(Tr{k}(1:3,1:3)); 
end 
pause(0.05); refresh; 


% output statistics 
num_matches = visualOdometryStereoMex ( 'num. matches ') ; 
num.inliers = visualOdometryStereoMex('num_inliers”); 
disp(['Frame: ' num2str(frame) 
>, Matches: ’ num2str(num.matches) 
^; Inliers: " num2str(100*«num.inliers/num. matches, '06.1£") ,* %”]); 


end 


% release visual odometry 
visualOdometryStereoMex( close’); 


Listing B.1: Matlab code running images from the stereo camera through the LIBVISO2 Matlab 
wrapper. 


C++ Code 


[** 


* 


This file is part of ORB-SLAM2. 


Copyright (C) 2014—2016 Raul Mur-Artal «raulmur at unizar dot es» (University of 
Zaragoza) 


x For more information see <https:// github.com/raulmur/ORB.SLAM2- 
* 

* ORB-SLAM2 is free software: you can redistribute it and/or modify 
* it under the terms of the GNU General Public License as published by 
* the Free Software Foundation, either version 3 of the License, or 
* (at your option) any later version. 

* 

* ORB-SLAM2 is distributed in the hope that it will be useful, 

* but WITHOUT ANY WARRANIY; without even the implied warranty of 

* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 

* GNU General Public License for more details. 

* 

* You should have received a copy of the GNU General Public License 
x along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>. 
* 

* This is a modification of stereo kitti.cc 

* 

*/ 

#include<iostream> 

#include<algorithm> 

#include<fstream> 


28 #include<iomanip> 
29 #include<chrono> 


30 


31 #include<opencv2/core/core.hpp> 
32 #include” opencv2/ccalib/omnidir.hpp” 


3 


i4 #include<System.h> 


36 using namespace std; 


i: void LoadImages(const 


11 int main(int argc, 


12 { 


cerr << endl << ” Usage: 


string &strPathToSequence , 
vector<string> &vstrImageRight , 


vector<string> &vstriImageLeft , 
vector<double> &vTimestamps) ; 


char **argv) 


13 if(arge != 4) 
{ 


./stereo_fisheye path_to_vocabulary 


path to settings path to sequence" << endl; 
return 1; 


47 H 


19 // Left camera parameters 
so double fx.0 = 519.3694647697515; 


51 double fy_0 = 519.0972993433265; 

52 double cx_0 = 606.9920690176958; 

53 double cy.0 = 614.5209527178968; 

54 double k1.0 — 0.005014375658016985; 

55 double k2.0 — —0.000775063814214897; 

56 double k3.0 = 0.0011170222555364636; 

57 double k4.0 = —0.0007862440518567923; 

5s CM Mati KO {even Vatx39 diim ORO coc OPE OC E) E QT 
s» cv::Mat distCoeffs O[cv::Matx4ld(k1.0, k2.0, k3.0, k4_0}}; 
60 

o1 // Right camera parameters 

62 double fx.1 — 519.5060341188239; 

63 double fy.1 — 519.221982350495; 

64 double cx.1 — 610.0999921090805; 

65 double cy-1 = 619.1931456774731; 

66 double k1.1 = 0.004427943702286365; 

67 double k2.1 — 0.0019943866353925744; 

6s double k3.1 = —0.0012327570856959112; 

69 double k4.1 = —0.00016670167097836632; 

Lco cde KCL eyes Mapas mx. 0.5 csi. O., Syll. cw ©, (55 E 
71 cv::Mat distCoeffs_l{cv::Matx41d{k1_1, k2.1, k3.1, k4_1}}; 


88 


/// FLIP Left and Right 
bool flip-intrinsics — false; 
if (flip.intrinsics) 


instrinsics 
//TODO: BE AWARE OF THIS SETTING! 


cv::Mat Ktmp = K 1.clone(); 

cv::Mat distCoeffs.tmp = distCoeffs_1.clone(); 
K_1 = K.0.clone(); 

distCoeffs.1 = distCoeffs_0.clone() ; 

K_0 = K_tmp.clone(); 

distCoeffs_0 = distCoeffs tmp.clone(); 


} 


// Pose of the left camera relative to the right camera 
cv::Matx44d pose right left {0.9999986808255548, 9.274992647641998e—06, 
0.001624272489956983, —0.15208456050238806, 
—4.886274970804611e—06, 0.9999963497357693, 
—4.399731155833786e—05, 
—0.0016242916214265777, 
—0.0001058955063797397, 


—0.002701942127105034, 
0.002701930626130018, 
0.9999950306114611, 


s 0.0, 0.0, 0.0, 1.0}; 

90 

91 ev:: Affine3d T(pose.right left }; 

o» // Output from stereoRectify for OpenCV 3.1 

94 cv :: Matx33d RO{0.9999972641707314, 0.0003004502449616822, 0.002319780313407229, 
)5 —0.0002973157448975248, 0.9999990426167138, —0.00135143220464351, 

96 —0.002320184130625263, 0.001350738800143851, 0.999996396118653); 

is cv :: Matx33d R1{0.9999997157419164, 0.0002892949744817117, 0.0006962934035986757, 


—0.0002902354630703445, 0.9999990453003514, 0.001350985477960567, 


48 


102 CV :: 


107 ENEE 


111 ENE 


116 CV :: 
117 CVELE 


151 // 


105 // 


168 


APPENDIX B. CODE LISTINGS 


—0.0006959019055382363, —0.001351187182970451, 0.9999988450062003}; 


Matx44d Q{1, 0, 0, —616.3594910271584, 
0, 1, 0, —635.2006218167855, 
0, 0, 0, 274.5918273014103, 
0, 0, 6.57528754029055, —0); 


Matx34d P0(274.5918273014103, 0, 616.3594910271584, 0, 
0, 274.5918273014103, 635.2006218167855, 0, 
0, OF Jl s 0}; 


: Matx34d P1{274.5918273014103, 0, 616.3594910271584, —41.76118924363826, 


0, 274.5918273014103, 635.2006218167855, 0, 
0, OF i, 0}; 


Mat mapl.0, map2.0, mapl.1, map2.1; 
fisheye::initUndistortRectifyMap(K.0,distCoeffs. 0 ,RO,P0,cv:: Size {1200 ,1200}, 
CV_32FC1,map1_0, map2.0); 


:: fisheye :: init UndistortRectifyMap (K-1, distCoeffs_1 ,R1,Pl,cv:: Size {1200,1200}, 


CV_32FC1,map1_1, map2_1); 


// Retrieve paths to images 

vector<string> vstrlmagebLeft ; 

vector<string> vstrIimageRight; 

vector<double> vTimestamps; 

LoadImages(string(argv[3]) , vstrImageLeft , vstrlmageRight, vTimestamps) ; 


const int nlmages = vstrlImageLeft.size(); 


// Create SLAM system. It initializes all system threads and gets ready to 


process frames. 
ORB.SLAM2:: System SLAM(argv [1] , argv [2] ,ORB.SLAM2: : System :: STEREO, true) ; 


// Vector for tracking time statistics 
vector<float> vTimesTrack ; 
vTimesTrack. resize (nImages) ; 


cout << endl << "—————” << endl; 
cout << "Start processing sequence ...” << endl; 
cout << "Images in the sequence: ” << nImages << endl << endl; 


// Main loop 

cv::Mat imLeft , imRight; 

cv::Mat im.0, im.1; 

for(int ni=0; ni<nImages; ni++) 

1 
// Read left and right images from file 
imLeft = cv::imread(vstrlImageLeft [ni] ,CV.LOADIMAGE. UNCHANGED) ; 
imRight = cv::imread(vstrImageRight [ ni], CV.LOAD.IMAGE.UNCHANGED) ; 
double tframe — vTimestamps [ni]; 


if (imLeft .empty() ) 


{ 
cerr << endl << "Failed to load image at: ” 
<< string (vstrimageLeft[ni]) << endl; 
return 1; 
j 


/// Undistort imLeft to get im-0 and imRight to get im-1 
cv::remap(imLeft , im_0, mapl.0,map2.0,cv :: INTER.LINEAR) ; 
cv::remap(imRight, im.1, mapl.1,map2 1,cv::INTER. LINEAR) ; 


/// Equalize histograms and median filter 
clahe—>apply (im-0,im-0); 

cv::equalizeHist(im-0,im-0); 

cv :: medianBlur(im.0 ,im.0,3) ; 


clahe—>apply (im_1,im_1); 
cv:: equalizeHist (im.1,im.1); 
cv :: medianBlur(im.1,im.1,3); 


169 #ifdef COMPILEDWITHC11 





// Pass the images to the SLAM system 
SLAM. TrackStereo(im-0,im-1,tframe); 


#ifdef COMPILEDWITHC11 


J 


void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft , 


{ 
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std ::chrono:: steady_clock::time_point tl = std::chrono:: steady_clock ::now() 
; 
#else 
std :: chrono :: monotonic.clock::time.point tl = std ::chrono:: monotonic.clock 
::now(); 
3 #endif 


Std::chrono::steady.clock::time.point t2 = std::chrono::steady.clock ::now() 
; 
#else 
std :: chrono :: monotonic.clock::time.point t2 = std ::chrono:: monotonic.clock 
::now(); 
| #eendif 


double ttrack= std::chrono::duration.cast«std::chrono::duration«double» »( 


t2 — t1).count() ; 
vTimesTrack [ ni]=ttrack ; 


// Wait to load the next frame 
double T=0; 
if (ni<nImages—1) 

T = vTimestamps | ni4+1]-tframe; 
else if(ni>0) 

T = tframe-vTimestamps [ni —1]; 


if (ttrack<T) 
usleep((T-ttrack)*1e6); 


i 


// Stop all threads 
SLAM. Shutdown () ; 


// Tracking time statistics 

sort (vTimesTrack. begin() , vTimesTrack.end()); 
float totaltime = 0; 

for(int ni=0; ni<nImages; ni++) 


{ 
J 


CONUM aroma << endl; 
cout << "median tracking time: ” << vTimesTrack[nImages/2] << endl; 
cout << "mean tracking time: ” << totaltime/nImages << endl; 


totaltime+=vTimesTrack[ni]; 


// Save camera trajectory 
SLAM. Save TrajectoryKITTI(” Camera Trajectory . txt") ; 


return O0; 


vector<string> &vstrlmageRight , vector<double> &vTimestamps ) 


ifstream fTimes; 
string strPathTimeFile = strPathToSequence + ”/times.txt”; 
fTimes.open(strPathTimeFile.c.str()); 
while (! f Times. eof ()) 
{ 
string s; 
getline (fTimes,s); 
if (!s.empty() ) 


stringstream ss; 

Ss == 5, 

double t; 

ss >> t; 

vTimestamps. push_back(t) ; 
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string strPrefixLeft = strPathToSequence + ”/left/”; 
string strPrefixRight = strPathToSequence + ”/right/”; 


const int nTimes = vTimestamps.size(); 
vstrilmageLeft. resize (nTimes) ; 


vstrImageRight . resize (nTimes) ; 


for(int i=0; i<nTimes; i++) 


{ 
stringstream ss; 
ss << setfill(’0’) << setw(6) << ix7; 
vstrimageLeft[i] = strPrefixLeft + ss.str() + ”.png”; 
vstrImageRight [i] = strPrefixRight + ss.str() + ".png"; 
j 


Listing B.2: C++ code running images from the stereo camera through ORBSLAM2. 


include "ros/ros.h" 
#include "std. msgs/String.h"? 


#include <sstream> 


5 #include <fstream> 


| #include <fentl.h> 


#include <string.h> 
#include <termios.h> 
#include <stdio.h> 

#include <iostream> 


int main(int argc, char x*x*argv) 
i 
ros::init(argc, argv, "talker"); 
ros :: NodeHandle n; 
ros:: Publisher chatter pub = n.advertise<std_msgs:: String >(”nmea” , 1000); 
ros::Rate loop_rate (10); 


// Open USB port 

// TODO Select port in arguments 

iat dA. 

fd = open(”/dev/ttyUSB1”, ORDWR | ONOCTIY | ONDELAY); 


if (fd = —1) 

printf (” Unable to open /dev/ttyUSBO. In”); 
j 

else 


{ 

fentl(fd, F.SETFL, 0); 

printf(” Port USBO is open. An”); 
J 


// Port settings 
// Default port settings work so this is not used 


[** 
struct termios port-settings; // structure to store the port settings in 
cfsetispeed(&port.settings , B9600) ; // set baud rates 


cfsetospeed(&port.settings , B9600); 


port_settings.c_cflag &— ~PARENB; // set no parity , stop bits, data bits 
port settings.c cflag &— ~CSTOPB; 

port.settings.c.cflag &— "CSIZE; 

pont settings canta [e Cu 


tcsetattr(fd, TCSANOW, &port.settings); // apply the settings to the port 
printi Eor wisi comic ume dae 


*/ 


sleep (2); 
tcflush (fd, TCIOFLUSH); // Flush USB port 


// Write commands to the GNSS receiver, see OEM6 firmware refrence manual for log 
description 

// TODO Select logs in arguments 

unsigned char str[]=”unlogalllr”; 

unsigned char str2[]=”log bestposa ontime 1\r”; 

unsigned char str3[]=”log headinga onchanged\r” ; 


//unsigned char str4[]=”nmeatalker auto\r”; 


write(fd, str, sizeof(str)); 
//write(fd, str4, sizeof(str4)); 
write(fd, str2, sizeof(str2)); 
write(fd, str3, sizeof(str3)); 


unsigned char buffer [492]; // Create buffer to store GNSS message 


while (ros::ok()) 
1 
sleep(1); // 1 Hz 


std. msgs::String msg; // Using sdt msg ROS message to publish the GNSS logs 
//TODO Use or write a more suitable message topic 
std::stringstream ss; 


read (fd , buffer ,492); // Read GNSS logs 
//TODO Split logs into different ROS topics 


ss << buffer; 
msg.data = ss.str(); 


memset(buffer , 0, sizeof(buffer)); // Clear buffer 
tcflush(fd, TCIOFLUSH); // Flush USB port 


chatter pub.publish(msg); // Publish message on ROS topic 
ros ::spinOnce() ; 
loop-rate.sleep(); 


} 


return 0; 


Listing B.3: ROS package that communicates with the FlexPak6D GNSS receiver. 
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